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Abstract 


Estimation  of  structure  and  six-degree-of-freedom  motion  of  manoeuvring  objects 
through  measurements  of  feature  positions  in  long,  multiple-camera  image  sequences 
is  widely  recognized  to  have  broad  industrial,  military,  and  space  applications,  par¬ 
ticularly  in  the  control  of  autonomous  systems.  This  report  focuses  on  the  case  of 
manoeuvring  objects  thereby  removing  restrictive  assumptions  concerning  the  mode 
of  translational  and  rotational  motion  which  are  commonly  employed  in  many  exist¬ 
ing  methods.  Object  manoeuvres,  being  “smooth”  and  time  correlated,  are  modelled 
as  first-order  Gauss- Markov  processes  for  both  translational  and  rotational  motion. 
In  the  literature,  rotational  motion  is  often  parameterized  with  unit  quaternions  even 
though  constraints  on  the  quaternion  norm  are  not  easily  enforced,  roll-pitch-yaw 
parameterizations  have  been  reported  to  be  poorly  behaved  and  have  led  to  com¬ 
putationally  demanding  implementations,  and  results  using  the  Euler  angle-axis  pa¬ 
rameterization  in  recursive  motion  and  structure  estimation  are  not  available.  In 
this  report,  six-degree-of-freedom,  nonlinear,  approximate  state  estimation  filters  for 
quaternion,  roll-pitch-yaw,  and  angle-axis  parameterizations  are  compared  in  terms 
of  estimation  performance  for  manoeuvring  object  trajectories.  Special  considera¬ 
tion  is  given  to  the  problem  of  imposing  unit  norm  on  the  estimated  quaternion 
since  previously  proposed  methods  led  to  filter  instability,  particularly  in  angular 
velocity  estimation.  Methods  proposed  herein  not  only  demonstrate  how  previous 
quaternion-based  algorithms  might  be  extended  to  track  manoeuvring  objects  ob¬ 
served  in  multiple-camera  image  sequences,  but  also  provide  two  much  simpler  alter¬ 
natives  using  angle-axis  and  roll-pitch-yaw  parameterizations.  The  angle-axis  filter 
was  found  to  give  the  best  overall  performance  with  a  significant  reduction  in  com¬ 
putational  requirements  in  comparison  to  the  quaternion  filter. 
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Executive  Summary 

Image-based  motion  analysis  is  primarily  concerned  with  accumulation  and  re¬ 
finement  of  information  related  to  motion  and  structure  of  objects  within  the  en¬ 
vironment  of  an  imaging  system.  Motion  parameters  include  position,  orientation, 
and  time  derivatives  of  position  and  orientation  of  objects  with  respect  to  a  specified 
reference  frame.  Structure,  in  the  present  context,  refers  to  the  shape  of  observed 
objects  in  the  sense  that  positions  of  object  features  are  estimated  with  respect  to 
an  object-centred  reference  frame.  Estimation  of  structure  and  six-degree-of-freedom 
motion  of  manoeuvrin'  bjects  through  measurements  of  feature  positions  in  long, 
multiple-camera  image  sequences  is  widely  recognized  to  have  broad  industrial,  mil¬ 
itary,  and  space  applications.  Image  sequence  andysis  provides  a  means  for  passive 
tracking  of  either  airborne  or  ground-baised  moving  targets.  Structure  estimation  in 
a  static  environment  and  recovery  of  motion  of  a  moving  platform  which  carries  the 
imaging  system  is  an  equivalent  problem  which  is  of  fundamental  importance  in  the 
control  and  guidjince  of  autonomous  systems. 

The  Defence  Technologies  Division  of  the  Defence  Research  Establishment  Suffield 
has  several  groups  whose  research  efforts  are  or  will  be  focussed  in  part  on  acquiring 
information  from  imaging  systems:  object  localization,  tracking  and  recognition  is  of 
primary  interest  to  the  Threat  Detection  Group;  methods  for  passive  target  tracking 
are  being  investigated  by  the  Systems  Integration  and  Vehicle  Concepts  Groups;  and 
vision-based  navigation  and  control  of  autonomous  systems  are  common  research 
areas  of  the  Advanced  Guidance  Concepts,  Electronic  Design  and  Instrumentation, 
and  Vehicle  Concepts  Groups. 

This  report  provides  a  framework  on  which  future  research  in  motion  and  structure 
estimation  can  be  based.  Methods  pro{>osed  herein  concentrate  primarily  on  the 
recovery  of  motion  and  structure  of  a  rigid  object  observed  with  a  multiple-camera 
imaging  system.  However,  the  resulting  state  estimation  algorithms  are  sufficiently 
general  to  be  of  interest  in  a  broad  range  of  applications,  including  those  of  the 
various  research  groups  listed  above.  The  report  provides  a  detailed  review  of  image 
analysis  methods  currently  available  in  the  literature  and  proposes  a  hierarchical 
image  analysis  system  which  partitions  the  overall  problem  into  three  coupled  target 
tracking  problems  with  increasing  levels  of  complexity.  One  of  these  levels  is  concerned 
with  the  estimation  of  structure  and  six-degree-of-freedom  motion  of  observed  objects 
and  is  the  focus  of  the  remainder  of  the  report.  Detailed  background  material  is 
provided  in  the  Appendices  in  order  that  this  report  may  be  self-contained. 

A  fundamental  difficulty  associated  with  vision-based  motion  estimation,  in  com¬ 
parison  to  the  more  traditional  radar-based  point  tracking  problem  for  example,  lies 
in  the  recovery  of  the  object’s  orientation  and  rotational  motion.  Theoretically  pre¬ 
cise  models  of  rotational  motion  may  be  formulated  in  terms  of  the  object’s  inertial 
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parameters,  external  torques  acting  on  the  object,  and  Euler’s  equations  which  repre¬ 
sent  a  coupled  system  of  three  nonlinear  differential  equations  that  admit  closed-form 
solutions  only  in  very  limited  cases.  In  object  tracking  applications,  however,  inertial 
parameters  and  external  torques  are  generally  not  available.  Consequently,  alternate 
and  often  simplified  models  for  rotational  motion  are  employed. 

Many  existing  methods  for  recursive  motion  and  structure  estimation  from  long 
image  sequences  have  been  developed  under  specific  assumptions  concerning  the  na¬ 
ture  of  translational  and  rotational  motion,  for  example,  constant  velocity,  accel¬ 
eration,  precession,  or  angular  momentum.  A  number  of  techniques  define  object 
structure  based  in  part  on  the  assumed  motion  of  the  object  which  can  lead  to  un¬ 
observability  for  some  manoeuvres.  Rotational  motion  is  often  modelled  with  unit 
quaternions  for  which  dynamical  models  are  parameterized  directly  in  terms  of  angu¬ 
lar  velocity  and  are  nonlinear.  Roll-pitch-yaw  parameterizations  have  been  reported 
to  be  poorly  behaved  numerically  and  have  led  to  computationally  demanding  imple¬ 
mentations.  Published  results  using  the  Euler  angle-axis  parameterization  in  recursive 
motion  and  structure  estimation  are  not  present  in  the  literature. 

Methods  proposed  herein  focus  in  part  on  removing  restrictive  assumptions  con¬ 
cerning  the  nature  of  object  motion  and  eliminating  dependence  of  object  structure 
on  assumed  motion  by  developing  motion,  structure,  and  measurement  models  for  a 
rigid  manoeuvring  object  observed  with  a  multiple-camera  imaging  system.  Object 
manoeuvres,  being  “smooth”  and  time  correlated,  are  modelled  as  first-order  Gauss- 
Markov  processes  for  both  translational  and  rotational  motion.  Object  structure  is 
defined  on  the  basis  of  observed  feature  points  only.  Six  degree-of-freedom  extended 
Kalman  filters  for  roll-pitch-yaw,  Euler  angle-axis,  and  quaternion  parameterizations 
are  developed  and  compared  in  terms  of  estimation  performance  for  manoeuvring 
object  trajectories. 

Both  the  angle-axis  and  roll-pitch-yaw  filters  are  based  on  approximate  linear 
dynamic  models  which  lead  to  computationally  efficient  implementations.  Computar 
tional  requirements  of  the  quaternion  filter  are  more  significant  than  the  other  two 
due  primarily  to  the  nonlinear  dynamic  model.  Another  difficulty  associated  with  the 
quaternion  filter  is  that  nonlinear  constraints  which  maintsun  unit  quaternion  norm 
are  not  easily  incorporated  into  the  linear  structure  of  the  Kalman  filter.  Previously 
proposed  methods  of  applying  an  impulsive  normalization  of  the  estimated  quaternion 
immediately  following  each  observation  event  was  found  to  contribute  significantly  to 
divergence  in  angular  velocity  estimates.  Instead,  the  quaternion  estimate  is  propa¬ 
gated  without  impulsive  normalization,  and  a  unit  quaternion  as  well  as  appropriately 
scaled  structure  vectors  are  extracted  from  the  state  estimate. 

Simulation  results  with  computer-generated  imagery  suggest  that  all  three  filters 
perform  equally  well  for  most  trajectories.  The  roll-pitch-yaw  filter  has  the  simplest 
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form,  but  sometimes  yields  poor  results  in  angular  velocity  estimates.  The  quaternion 
filler  shows  degraded  performance  in  estimation  of  object  position  ^lnd  some  structural 
parameters.  The  angle- axis  filter  is  computationally  efficient  and  performs  at  least  as 
well  as,  and  often  better  than,  the  quaternion  and  roll-pitch-yaw  filters.  This  work  not 
only  demonstrates  how  previously  proposed  quaternion- based  approaches  might  be 
extended  to  track  m«moeuvring  objects  observed  in  multiple- camera  image  sequences, 
but  also  provides  two  much  simpler  alternatives  using  angle-axis  and  roll-pitch-yaw 
parameterizations.  The  angle-axis  filter  b«ised  on  a  linear  dynamic  model  was  found 
to  give  the  best  overall  performeince  with  a  significant  reduction  in  computational 
requirements  in  comparison  to  the  quaternion  filter. 

Future  research  efforts  will  address  problems  associated  with  implementation  of 
other  modules  of  the  hierarchical  structure  proposed  in  Section  1.2.  Generally,  the 
proposed  system  is  composed  of  three  coupled  multi-target  tracking  systems  with  in¬ 
creasing  levels  of  complexity.  It  might  be  expected  that  statistical-based  multi-target 
tracking  methods,  which  have  been  extensively  studied  in  radar-based  point  track¬ 
ing  applications,  will  be  extremely  useful  in  such  a  system.  Feature  detection  and 
correspondence  as  well  as  feature  occlusion  processes  represent  challenging  problems 
for  future  investigations.  However,  a  primary  consideration  in  future  work  will  also 
involve  further  evaluation  of  the  extended  Kalmain  filtering  approach  in  comparison 
to  other  nonlinear  observers.  Adaptive  extended  Kalman  filtering,  Lyapunov  meih- 
ods,  transformation  to  nonlinear  observer  canonical  form,  nonlinear  map  inversion, 
and  sliding  mode  observers  are  representative  of  recently  proposed  methods  which 
may  lead  to  more  robust  observers  for  recovery  of  object  motion  and  structure  from 
multiple-camera  image  sequences.  Initially,  future  work  will  focus  on  performance 
evaluations  with  simulated  imagery.  As  motion  and  structure  estimation  algorithms 
mature,  however,  experiments  with  real  imagery  will  be  required  to  assess  the  true 
performance  of  proposed  image  analysis  systems. 
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1.  Introduction 


A  primary  function  of  biological  vision  systems  is  the  detection,  processing,  and 
understanding  of  motion  perceived  from  a  continuous  flow  of  visual  information.  Per¬ 
ceived  motion  results  from  relative  motion  of  a  vision  system  within  a  dynamic  en¬ 
vironment.  Analysis  of  object  motion  in  scenes,  or  the  apparent  motion  perceived 
by  a  moving  observer  in  a  static  scene  can  yield  vital  information  concerning  the 
structure  and  dynamic  behavior  of  the  observed  process.  Over  the  last  two  decades, 
research  in  motion  detection,  analysis,  and  understanding  techniques  with  applica¬ 
tions  in  computer  vision  systems  for  robotics  and  autonomous  vehicles  represents 
an  area  of  vigorous  growth.  Rosenfeld's  annual  bibliographical  surveys  (see  [1]  for 
example)  demonstrate  the  intense  interest  in  this  field  of  research. 

Motion  and  structure  estimation  from  multiple-camera  image  sequences  has  broad 
industrial,  military,  and  space  applications.  Motion  parameters  include  {>osition,  ori¬ 
entation,  and  time  derivatives  of  position  and  orientation  of  an  object  with  respect  to 
a  specified  reference  frame.  Structure,  in  the  present  context,  refers  to  the  shape  of 
the  observed  object  in  the  sense  that  feature  positions  are  estimated  with  respect  to 
an  object-centred  reference  frame.  These  notions  are  made  more  precise  in  Chapters  2 
and  3.  A  fundamental  difficulty  associated  with  vision-based  motion  estimation,  in 
comparison  to  the  more  traditional  radar-based  point  tracking  problem  for  example, 
lies  in  the  recovery  of  object  orientation  and  rotational  motion.  Theoretically  precise 
models  of  rotational  motion  may  be  formulated  in  terms  of  the  object’s  inertial  pa¬ 
rameters,  external  torques  acting  on  the  object,  and  Euler’s  equations  which  represent 
a  coupled  system  of  three  nonlinear  differential  equations  that  admit  closed-form  so¬ 
lutions  only  in  very  limited  cases.  In  object  tracking  applications,  inertial  parameters 
and  external  torques  are  generally  not  avulable.  Consequently,  alternate  and  often 
simplified  models  for  rotational  motion  are  employed. 

Many  existing  approaches  to  motion  and  structure  recovery  from  long  image  se¬ 
quences  are  somewhat  restrictive  in  that  solutions  are  proposed  for  specific  cases  of 
translational  and  rotational  motion,  for  example,  constant  velocity,  acceleration,  pre¬ 
cession,  or  angular  momentum,  etc.  These  techniques  have  been  designed  and  evalu¬ 
ated  on  appropriate  data  generated  under  these  assumptions.  However,  such  precise  a 
priori  information  is  seldom  available  in  general  object  tracking  applications,  particu¬ 
larly  in  an  unstructured  environment.  Rotational  motion  is  often  parameterized  with 
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unit  quaternions,  which  leaids  to  further  difRculties  when  enforcing  a  nonlinear  con¬ 
straint  on  the  quaternion  norm.  Roll-pitch-yaw  parameterizations,  on  the  other  hand, 
have  been  reported  to  be  neither  simple  nor  well-behaved  numerically  and  have  led 
to  overly  complicated  and  computationally  demanding  implementations.  The  Euler 
angle-axis  parameterization  has  appeared  in  small  intersample-angle  approximations, 
but  published  results  in  the  context  of  recursive  motion  and  structure  estimation  a^e 
not  avciilable.  None  of  these  methods  have  been  develop>ed  for  the  case  of  manoeu¬ 
vring  objects  with  measurements  from  multiple-camera  systems.  These  observations 
are  discussed  further  in  the  literature  review  of  Section  1.1.  The  reader  who  might 
be  unfamiliar  with  these  parameterizations,  after  becoming  familicir  with  notation 
introduced  in  Chapter  2,  may  wish  to  consult  Appendix  A  which  presents  a  brief 
tutorial  review. 

Primary  contributions  of  the  research  reported  herein  include  the  development  of 
motion,  structure,  and  measurement  models,  and  corresponding  recursive  nonlinear 
filters  to  estimate  motion  and  structure  of  a  manoeuvring  object  through  measure¬ 
ments  taken  from  a  multiple-camera  imaging  system.  Six  degree-of- freedom,  non¬ 
linear,  approximate  state  estimation  filters  for  three  parameterizations  of  rotational 
motion,  using  roll-pitch-yaw,  Euler  angle-axis,  and  quaternion  parameters,  are  devel¬ 
oped  and  compared  in  terms  of  estimation  performance  for  manoeuvring  object  tra¬ 
jectories.  Simulation  studies  have  been  conducted  over  a  wide  range  of  trajectories. 
Simulation  results  suggest  that  the  roll-pitch-yaw,  Euler  angle-axis,  and  quaternion 
filters  perform  equally  well  in  almost  all  cases,  with  the  roll-pitch-yaw  filter  giving 
slightly  poorer  results  and  the  angle-axis  filter  giving  slight  improvements  over  the 
the  other  two.  The  roll-pitch-yaw  filter  has  the  simplest  form  due  to  the  use  of  a 
linear  dynamic  model.  The  Euler  angle-axis  filter  is  also  derived  from  a  linear  dy¬ 
namic  model  which  results  in  computationally  efficient  implementation,  but  requires 
an  occasional  impulsive  reset  of  the  state  estimate  to  m^ntain  the  rotation  angle  to 
within  ±x.  The  quaternion  filter  is  the  most  computationally  demanding  of  the  three 
due  to  a  nonlinear  dynamic  model.  Time  propagation  of  the  estimated  quaternion  is 
performed  with  a  simple  numerical  integration  scheme  which  incorporates  estimates 
of  higher-order  time  derivatives  of  angular  velocity.  S]>ecial  consideration  is  given 
to  the  problem  of  imposing  unit  norm  on  the  estimated  quaternion,  since  previously 
proposed  methods  led  to  filter  instability,  particularly  in  angular  velocity  estimation. 
Strict  assumptions  are  not  imposed  on  the  mode  of  translational  or  rotational  motion 
except  “smoothness”  conditions  in  the  sense  that  par2uneters  may  be  differentiated 
with  respect  to  time. 

This  introductory  chapter  continues  with  a  discussion  of  motion  analysis  tech¬ 
niques  through  a  review  of  the  literature.  Section  1.1  describes  and  compares  two 
fundamentally  different  approaches  to  motion  analysis  known  as  image  flow-based  and 
feature-based  techniques,  and  presents  a  summary  of  major  contributions  to  motion 
analysis  which  are  based  on  extended  Kalman  filtering.  Section  1.2  discusses  vari¬ 
ous  components  of  feature-based  motion  analysis  and  proposes  a  possible  hierarchical 
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structure  for  an  image  analysis  system.  Three-dimensional,  six  degree-of-freedom 
motion  and  structure  estimation,  which  is  the  prim2iry  subject  of  this  investigation, 
represents  only  a  single  component  of  the  hierarchical  system.  Section  1.3  summarizes 
this  introductory  chapter  and  provides  an  overview  of  the  remainder  of  the  report. 


1.1  Literature  Review 

E^rly  work  in  image  sequence  analysis  prior  to  1978  has  been  surveyed  by  Martin 
and  Aggaxwal  [2]  and  Nagel  [3].  A  large  amount  of  research  during  this  period  fo¬ 
cussed  on  applications  of  motion  analysis  to  bandwidth  compression  for  transmission 
of  TV  signals,  and  on  detection  and  tracking  of  geographical  landmarks  and  cloud 
motion  in  satellite  imagery.  In  a  more  limited  sense,  real-time  feature  tracking  tech¬ 
niques  were  also  studied  in  applications  of  visual  feedback  to  industrial  automation 
and  autonomous  vehicle  navigation.  Feature  tracking  methods  initially  focussed  on 
extracting  motion  parameters  through  detection  of  object  displacement  between  two 
consecutive  frames — a  technique  that  is  still  being  investigated  today.  Some  empha¬ 
sis,  however,  shifted  towards  tracking  features  within  a  sequence  of  more  than  two 
image  frames  through  stochastic  modeling  of  the  dynamic  processes  being  observed. 

Motion  analysis  techniques  are  often  classified  according  to  whether  they  are  for¬ 
mulated  through  an  image  flow-based,  or  feature-based  approach.  The  image  flow 
approach  first  estimates  apparent  velocities  of  intensity  patterns  in  an  image  sequence 
before  motion  analysis  begins.  In  feature-based  algorithms,  features  such  as  points, 
lines,  contours,  corners,  etc.,  are  extracted  from  each  image  and  matched  between 
images  in  order  to  estimate  relative  motion  of  features  in  scenes  with  respect  to  an 
observer.  More  recent  surveys  have  been  presented  by  Nagel  [4],  who  focuses  on  tech¬ 
niques  based  on  measurement  of  image  flow,  and  Aggarwal  [5],  who  treats  primarily 
feature-based  techniques.  Feature  matching  between  successive  images  is  known  as 
the  correspondence  problem  and  has  been  reviewed  by  Aggarwal,  Davis,  and  Martin 
[6].  The  correspondence  problem,  although  it  has  not  yet  been  explicitly  treated  in 
this  research,  is  discussed  further  in  Section  1.2  in  terms  of  its  placement  in  the  overall 
motion  analysis  problem. 

Image  flow  is  often  modeled  by  the  image  flow  constraint  equation  [8]  which  de¬ 
scribes  the  interaction  between  velocity  fields  and  spatial  and  temporal  gradients  of 
image  intensity  functions  under  a  common  assumption  of  small  motion  between  suc¬ 
cessive  image  frames.  This  approach  requires  that  spatially  and  temporally  discrete 
intensity  distributions  be  differentiated  through  approximate  differencing  techniques 
which  tends  to  enhance  noise  present  in  real  images.  The  image  velocity  field  is  then 
iteratively  computed  for  each  pixel  based  on  various  proposed  optimization  criteria 
[8, 9, 13].  The  image  flow  field  contains  information  concerning  relative  depth,  surface 
orientation,  and  relative  motion  of  objects  [10].  Several  methods  have  been  proposed 
to  extract  three  dimensional  structure  and  motion  from  flow  fields  [11,  12,  14].  It 
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hcis  been  noted,  however,  that  the  system  of  equations  relating  position  and  motion 
in  space  to  image  position  and  flow  are  not  well  behaved  numerically  and  are  highly 
sensitive  to  noise  in  image  position  and  flow  estimates  [5].  Further  difficulties  are 
encountered  in  the  accurate  measurement  of  flow  fields  near  discontinuities  in  image 
intensity  and  in  areas  of  uniform  intensity  [8].  Moreover,  if  the  inter-frame  motion 
is  indeed  small,  inaccuracies  in  flow  field  estimates  may  overshadow  the  true  velocity 
field  and  lead  to  ambiguous  results. 

Feature-bcised  approaches  formulate  systems  of  equations  relating  positions  of  fea¬ 
tures  detected  in  an  image  sequence  to  object  translational  and  rotational  parameters. 
This  approach  implicitly  assumes  that  the  object  of  interest  has  a  set  of  discernible 
features  which  can  be  detected  in  the  image  sequences.  Roach  and  Aggarwall  [15]  give 
a  thorough  discussion  of  this  problem  when  only  two  or  three  frames  are  considered. 
They  proposed  a  modified  least-squared  error  solution  of  a  nonlinear  system  of  equa¬ 
tions  to  estimate  motion  parameters.  Their  results  demonstrate  that  this  approach 
can  be  quite  sensitive  to  noise  in  estimates  of  feature  positions  in  the  image.  Tsai  and 
Huang  [16, 17]  have  shown  that  in  many  cases  motion  parameters  can  be  obtained  by 
solving  a  system  of  linear  equations  for  a  set  of  “essential  parameters”,  followed  by 
a  Singular  Value  Decomposition  of  the  3x3  essential  parameter  matrix.  Their  sim¬ 
ulation  results  also  show  that  motion  parameter  estimates  can  be  highly  sensitive  to 
feature  position  errors.  Fang  and  Huang  [18]  have  implenoented  the  approach  and  give 
detailed  experimental  results.  Again,  accuracy  of  the  results  are  seriously  degraded 
in  the  presence  of  feature  position  measurement  errors.  Recently,  extensions  of  this 
method,  such  as  those  proposed  by  Philip  [22]  or  Weng  et  al  [19,  20,  21]  for  exam¬ 
ple,  aim  at  reducing  measurement  noise  sensitivity  through  refinement  of  parameter 
estimates  over  multiple  frames. 

Structure  and  motion  processes,  and  the  correspondence  process  axe  believed  to  be 
complementary,  perhaps  even  inseparable,  in  the  human  visual  system  [73].  Feature 
detection  and  correspondence  are  required  to  interpret  raw  image  data  and  drive  the 
motion  estimation  scheme.  Knowledge  of  motion  parameters  and  their  uncertainty, 
on  the  other  hand,  may  serve  as  constraints  to  reduce  the  search  space  associated 
with  detection  and  correspondence  problems.  Matthies  and  Kanade  [43]  give  a  thor¬ 
ough  discussion  of  this  concept  and  provide  two  illustrative  examples  which  propose 
solutions  based  on  Kalmam  filtering  techniques.  The  Kalman  filter  with  its  recursive 
structure  and  explicit  modeling  of  uncertiunty  has  been  proposed  as  an  extremely  use¬ 
ful  approach  to  dynamic  vision  problems,  particularly  in  view  of  the  “built  in”  state 
and  error  prediction  capabilities.  Applications  of  Kalman  filtering  to  image  analysis 
have  been  reported  by  many  authors,  for  example  Futrelle  and  Speckert  [23],  Gennery 
[24],  Hallman  [25],  Legters  and  Young  [26],  Stuller  and  Krishnamurthy  [27],  Wu  tt  al. 
[38],  Matthies,  Kanade,  and  Szeliski  [44],  Chang  et  al.  [39],  and  lu  and  Wohn  [53,  54] 
consider  diverse  vision-based  problems  to  which  sequential  state  estimation  has  been 
applied.  The  work  of  three  other  groups,  however,  provides  the  primary  motivation 
for  the  approach  investigated  in  this  report. 
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Ayache  and  Faugeras  [40],  Ayache  and  Lustman  [41],  Randall,  Foret,  and  Ay- 
ache  [42]  (see  further  references  cited  therein)  recursively  build  and  update  three 
dimensional  representations  of  the  environment  of  a  mobile  robot.  These  methods 
are  developed  primarily  for  trinocular  (three  cameras)  image  sequences,  however  their 
general  approach  could  also  be  applied  to  binocular  stereo  imagery  or  that  produced 
by  a  monoculjir  imaging  system  under  egomotion.  Positions  of  world  features  (points, 
lines,  planes)  are  recursively  estimated  from  measurements  in  the  image  plane  through 
extended  Kalman  filtering.  They  have  generalized  the  problem  of  updating  positions 
of  any  feature  so  that  the  same  Kalm2in  filtering  structure  can  be  employed  in  all 
cases.  Geometric  relations  are  inferred  between  features  by  computing  a  generalized 
Mahalanohis  distance  and  using  a  acceptance  test.  An  estimate  of  robot  motion 
is  also  maintained  with  the  Kalman  filter.  They  have  applied  their  ^^hnique  to  real 
scenes  taken  from  an  indoor  mobile  platform  and  demonstrate  some  of  the  major 
properties  of  their  algorithm. 

Dickmanns  and  Graefe  [45]  present  a  thorough  discussion  of  their  approach  to 
dynamic  machine  vision  which  employs  special  hardware  and  methods  for  feature 
extraction  and  information  processing.  Through  the  use  of  integral  spatio-temporal 
models,  extended  Kalman  filtering,  and  prediction  error  feedback,  they  have  demon¬ 
strated  real-time  vision-based  control  capabilities  in  several  important  problems  in 
robotics  [46].  Real-time  capability  in  dynamic  vision  is  achieved  through  a  special¬ 
ized  multiple  instruction  stream,  multiple  data  stream  (MIMD)  computer  architec¬ 
ture  [46,  47].  This  system  associates  each  processing  element  with  a  dynamically 
allocated  window  on  each  image  which,  in  turn,  is  associated  with  a  single  feature 
of  interest.  Each  window  is  defined  by  prediction  error  feedback  from  the  Kalman 
filter.  Controlled  correlation  techniques  are  employed  by  each  processing  element  to 
efficiently  extract  the  feature  of  interest  in  each  window.  Results  of  feature  extraction 
are  passed  from  all  processing  elements  to  a  host  computer  which  executes  the  ex¬ 
tended  Kalman  filter.  They  have  successfully  applied  their  technique  to  control  of  an 
inverted  pendulum  on  a  moving  cart  [48],  autonomous  vehicle  docking  problems  [46], 
autonomous  road  vehicle  guidance  [46],  and  vision-based  control  of  an  aircraft  in  a 
simulated  landing  approach  [49].  More  recently,  enhancement  of  road  curvature  mod¬ 
els  and  extensions  of  algorithms  and  hardware  to  obstacle  avoidance  in  autonomous 
vehicle  guidance  applications  have  been  proposed  [50,  51,  52]. 

Broida  [37],  Broida  and  Chellappa  [28,  29,  30,  31,  36],  Broida,  Chandrashekhar, 
and  Chellappa  [32],  and  Young  and  Chellappa  [33,  34,  35]  have  applied  extended 
Kalman  filtering  techniques  to  estimation  of  motion  and  structural  parameters  of  a 
rigid  object  through  measurements  taken  from  noisy  monocular  and  stereo  image  se¬ 
quences.  Early  work,  [28],  considered  a  special  case  of  the  general  motion  problem 
in  which  object  translationad  motion  is  constrained  to  lie  on  a  plane  defined  by  the 
optical  axis  and  central  scan  line  of  the  imaging  system.  All  feature  points  of  interest 
are  assumed  to  lie  on  this  plane.  E)stimates  of  model  parameters  were  refined  over 
an  arbitrary  number  of  image  frames  with  an  iterated  extended  Kalman  filter  under 
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the  assumption  that  some  prior  knowledge  of  object  structure  is  available.  In  three 
dimensional  tracking,  [29]-[32],  rotational  motion  is  modelled  with  unit  quaternions. 
An  earlier  choice  of  Euler  angles  and  rates  was  described  as  being  neither  simple 
nor  well-behaved,  which  is  in  sharp  contrast  to  the  results  reported  herein.  Several 
cases  have  been  treated  based  on  a  priori  information  concerning  the  nature  of  object 
motion:  pure  translational  motion,  [29];  constant  translational  and  rotational  ve¬ 
locities,  [32,  36);  and  constant  translational  acceleration  and  rotational  motion  with 
constant  precession,  [33,  34].  Both  recursive  algorithms,  [28,  29,  32,  33,  34],  and  a 
batch  approach  based  on  a  fixed  number  of  frames,  [30,  36],  have  been  investigated. 
Approximate  Cramer-Rao  covariance  bounds  in  the  absence  of  a  priori  information 
are  used  extensively  for  performance  analysis,  [31].  Considerable  effort  has  also  been 
devoted  to  uniqueness  issues  for  the  constant  acceleration  and  precession  case,  [34]. 
A  more  detailed  review  of  their  recursive  methods  for  monocular  systems,  [28,  32],  is 
given  by  Aitken  [58]. 

Methods  reviewed  above  have  been  developed  for  significantly  diverse  applications. 
Each  method  is  dependent  on  a  particular  set  of  assumptions.  For  example,  diiTerent 
types  of  imaging  sensors  have  been  investigated:  sonar  [25],  visual  monocular  [23]  [26]- 
[32]  [38, 39, 44, 45,  50],  or  visual  stereo  and  trinocular  [24, 33, 34,  40, 41, 45, 50].  Some 
approaches  assume  a  static  environment  with  moving  sensor  [25,  40,  41,  44],  while 
others  consider  dynamic  environments  with  stationary  sensors  [23,  24,  26]  [28]- [39], 
and  some  consider  both  a  dynamic  environment  and  moving  sensor  [27,  45,  50].  In 
some  cases  only  motion  parallel  to  the  image  plane  is  considered  [26,  27, 44].  Of  those 
techniques  which  estimate  motion  and  orientation  of  objects  in  scenes,  some  assume 
partial  or  complete  knowledge  of  object  structure  [23, 24, 28, 38]  while  others  estimate 
total  object  structure  from  stereo  images  or  with  monocular  image  sequences  assuming 
depth  information  is  available  [32,  33,  34).  A  general  result  is  that  performance  of 
the  above  techniques  cannot  be  directly  compared  since  each  must  be  applied  to  the 
specific  problem  for  which  it  was  developed. 

Evaluation  of  proposed  estimation  schemes  for  long  image  sequences  using  real 
imagery  has  been  conducted  extensively  only  in  the  work  of  Ayache  and  Faugeras 
[40],  Matthies  et  al.  [44],  and  Dickmans  et  al.  [46].  In  a  more  limited  sense,  Wu  ti  al. 
[38]  show  results  for  imagery  obtuned  in  a  controlled  laboratory  setting,  while  Broida 
[37]  and  Broida  et  al.  [32,  36]  show  qualitative  results  based  on  real  imagery  for  which 
ground  truth  data  is  not  available,  and  feature  points  were  extracted  manually.  In 
most  of  the  above  investigations,  simulations  are  performed  in  which  feature  detection 
and  correspondence  problems  are  assumed  to  be  solved.  In  this  case,  feature  positions 
in  each  image  are  obtained  from  true  analytical  motion  and  structural  models  and  an 
assumed  image  formation  model,  and  additive  noise  processes  are  included  to  simulate 
measurement  noise. 

Application  of  sequential  state  estimation  techniques  such  as  the  Kalman  filter 
to  machine  vision  problems  emphasizes  temporal  continuity  conditions  for  image  se¬ 
quence  interpretation.  This  allows  proper  definitions  of  state  variables  and  formula- 
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tion  of  systems  of  differential  equations  to  model  motion  processes  of  objects  in  space 
and  time.  Dickmanns  [45]  refers  to  spatio-temporal  motion  analysis  as  “dynamic  vi¬ 
sion”  which,  in  contrast  to  “static”  image  sequence  processing,  does  not  separately 
apply  object  recognition  from  one  frame  to  the  next  as  a  first  step  and  motion  recon¬ 
struction  (ifterwards  as  a  second.  Inste<id,  object  structure  and  motion  are  treated 
simultaneously  in  order  to  generate  an  estimate  of  the  system  state  based  on  noisy 
measurements  from  images.  There  are  several  immediate  advantages  of  this  approach: 

1.  Storage  requirements  for  past  images  are  reduced  or  eliminated  in  that  the 
current  system  state  can  be  represented  solely  by  slate  vectors  and  their  uncer¬ 
tainty,  together  with  shape  descriptors  which  describe  the  particular  feature  to 
which  each  state  vector  applies  (point,  line,  plane,  etc.); 

2.  The  motion  estimation  process  need  not  be  explicitly  dependent  on  assumptions 
of  small  motion  between  scenes  which  is  common  to  the  optical  flow  based 
approach  [8,  9,  11,  12,  13]; 

3.  The  estimation  process  can  be  made  robust  to  large  measurement  errors  through 
explicit  modeling  of  measurement  uncertsunty;  and 

4.  Feedback  of  state  predictions  and  uncertainties  reduces  the  search  space  asso¬ 
ciated  with  feature  detection  and  correspondence  problems. 

Two  disadvantages  of  this  technique  arise  from  computational  requirements  of  Kalman 
filters,  and  nonlinear  dynamical  and  measurement  models  which  result  in  the  use  of 
suboptimal  extended  Kalman  filters.  The  computational  burden  may,  in  some  cases, 
be  less  than  that  required  for  iterative  approaches  to  obtain  solutions  to  systems  of 
nonlinear  equations.  The  use  of  suboptimal  extended  Kalman  filters  in  computer 
vision  and  other  nonlinear  observer  applications  is  often  criticized  for  several  reasons: 

1.  Guaranteed  convergence  or  stability  cannot  be  established  a  priori; 

2.  Dynamical  models  are  often  approximated  as  polynomials  in  time  which  may 
contribute  to  modelling  error  and  instability;  and 

3.  Theoretical  requirements  of  precise  stochastic  models,  such  as  white  Gaussian 
disturbances  with  known  mean  and  covariance,  are  often  violated. 

The  fact  remains,  however,  that  extended  Kalman  filters  are  being  used  in  a  wide 
range  of  applications  with  varying  degrees  of  success.  In  these  applications,  extended 
Kalman  filters  should  be  considered  as  nonlinear  observers  with  a  particular  structure 
in  which  the  plant-,  and  in  some  cases  the  measurement-,  “covariance”  matrices  are 
treated  as  tuning  parameters  which  are  specified  by  the  designer  in  order  to  achieve 
a  desired  p>erformance  over  a  sufficient  range  of  trajectories. 
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In  planar  motion  applications,  the  socis  of  rotation  is  fixed  and  is  often  assumed  to 
be  known  [15,  25,  26,  28,  58,  59].  Parameterization  in  terms  of  angular  velocity  then 
results  in  linear  dynamical  models.  More  general  rotational  motion  is  often  modelled 
with  unit  quaternions  [24,  53,  55],  [29]  [37].  Dynamical  models  which  govern  time 
propagation  of  the  unit  quaternion  we  parameterized  in  terms  of  angular  velocity 
and  au'e  nonlinear.  State  estimation  is  then  subject  to  a  nonlinear  constraint  which 
imposes  unit  length  on  the  four-element  quaternion.  This  constraint  is  not  easily 
incorporated  into  the  linear  structure  of  the  Kalman  filter.  A  common  approach, 
[53],  [29]- [37],  is  to  normalize  the  estimated  quaternion  following  each  measurement 
update  of  the  filter.  However,  in  the  present  investigation  with  manoeuvring  objects, 
this  practice  was  found  to  contribute  significantly  to  divergence  in  angular  velocity  es¬ 
timates.  Bar-Itzhack  and  Oshm<in  [65]  also  observed  poor  performance  with  impulsive 
normalization  even  wheu  angular  velocity  measurements  are  available.  An  approach 
similar  to  that  employed  by  Bar-Itzhack  and  Oshman,  [65],  is  used  in  that  the  quater¬ 
nion  estimate  is  propagated  in  the  filter  equations  without  impulsive  normalization. 
For  output  purposes  only,  a  unit  quaternion  as  well  as  object  structure  vectors  are 
extrticted  from  the  state  estimate.  This  problem  is  discussed  further  in  Section  3.5.2. 
The  use  of  higher-order  time  derivatives  of  angular  velocity  in  a  quaternion-based 
filter  has  been  examined  by  lu  and  Wohn  [53]  who  employ  a  dynamical  model  for 
monocular  image  sequences  similar  to  Broida  et  al.  [32,  36].  However,  time  propa¬ 
gation  in  their  approach  assumes  constant  angular  velocity  over  the  sample  period. 
Methods  proposed  herein  assume  instead  that  the  highest  time  derivative  of  angular 
velocity  included  in  the  state  vector  is  approximately  constant  over  any  sample  pe¬ 
riod.  A  numerical  integration  scheme  which  mfuntuns  constant  quaternion  norm  is 
derived  for  time  propagation  of  the  quaternion  estimate. 

Quaternions  are  often  selected  over  roll-pitch-yaw  angles  due  to  mathematical  sin¬ 
gularities  of  the  roll-pitch-yaw  parameterization.  Wu  et  ai,  [38]  use  roll-pitch-yaw 
angles  sold  parameterize  rotational  motion  in  terms  of  a  constant  inter-frame  rota¬ 
tion  matrix.  This  approach  leads  to  an  extremely  complicated  and  computationally 
demanding  implementation.  Their  technique  has  been  applied  to  real  scenes  gen¬ 
erated  in  a  laboratory  environment  using  14  feature  points  on  an  object  of  known 
structure  which  is  constrained  to  lie  on  a  planar  surface.  Previous  research,  [58],  has 
demonstrated  that  very  little  additional  information  is  provided  to  the  planar  mo¬ 
tion  problem  as  the  number  of  feature  points  is  increased  beyond  4.  In  the  present 
investigation,  both  the  roll-pitch-yaw  and  Euler  angle-axis  filters  are  developed  from 
approximate  linear  dynamical  models.  In  this  case  rotational  motion  is  not  parame¬ 
terized  directly  in  terms  of  angular  velocity  and  its  time  derivatives.  Instead,  angular 
velocity  is  computed  through  a  nonlinear  function  of  the  state  estimates  and  compared 
to  the  true  value  and  that  generated  by  the  quaternion  filter. 

This  Section  has  provided  a  review  of  only  a  small  subset  of  literature  available 
on  motion  and  structure  estimation  and  has  focussed  on  those  methods  which  employ 
extended  Kalman  filtering.  Three  dimensional  motion  and  structure  estimation  repre- 


UNCLASSIFIED 


DRES-SR-577 


UNCLASSIFIED 


9 


sents  only  a  single  component  of  the  overall  motion  analysis  problem.  The  following 
Section  discusses  a  possible  structure  for  a  motion  analysis  system  and  describes 
some  of  the  assumptions,  restrictions,  and  limitations  of  the  methods  studied  in  the 
remainder  of  this  report. 


1.2  Hierarchical  Motion  Analysis 

The  motion  analysis  problem  is  primarily  concerned  with  accumulation  and  re¬ 
finement  of  information  pertaining  to  the  motion  and/or  structure  of  objects  within 
the  environment  of  an  imaging  system.  One  possible  hierarchical  approach  to  feature- 
based  motion  analysis  is  shown  in  Figure  1.1.  The  system  has  been  divided  into  eight 
levels,  as  indicated  on  the  right,  in  which  higher  levels  represent  increased  sophis¬ 
tication  in  the  representation  of  observed  processes.  In  Level  I  (bottom  row  of  the 
Figure),  multiple  cameras,  which  need  not  be  stationary,  observe  a  dynamic  scene. 
An  estimate  of  each  camera’s  motion  is  computed  from  local  sensors  and  p>erhaps 
feedback  from  higher  levels  of  the  system.  In  Level  II,  each  camera  produces  an 
image  sequence  which  must  be  captured  and  digitized,  and  may  pass  through  a  pre¬ 
processing  stage.  Feature  detection  algorithms  are  then  applied  in  Level  III  to  each 
frame,  or  a  subsequence  of  frames,  in  order  to  extract  features  of  interest  (points, 
lines,  corners,  etc.).  Feedback  from  motion  estimation  algorithms  in  higher  levels 
of  the  system  may  significantly  reduce  the  computational  effort  required  for  feature 
detection.  It  may  also  be  necessary  to  scan  any  remaining  portions  of  image  frames 
for  new  features,  or  features  that  might  reappear  after  being  occluded.  The  output 
data  from  Level  III  includes  sets  of  available  features  from  each  image  sequence. 

Level  IV  of  the  system  attempts  to  establish  correspondences  between  features 
extracted  from  image  sequences.  In  a  temporal  sequence  of  images  generated  by 
any  single  camera,  the  process  of  tracking  features  from  one  sample  time  to  the 
next  will  be  referred  to  as  “temporal  correspondence.”  In  the  case  of  feature  points, 
this  is  essentially  a  two-dimensional  multi-target  tracking  problem.  Salari  and  Sethi 
[57]  have  recently  proposed  a  batch  approach  for  temporal  correspondence  in  the 
presence  of  occlusion.  This  method  is  an  extension  of  that  prop>osed  by  Sethi  and 
Jain  [56].  However,  their  approach  is  based  on  the  assumption  that,  “As  long  as 
these  points  are  on  physical  objects,  we  can  safely  assume  that  the  trajectories  of 
these  points  in  the  stationary  image  plane  will  be  smooth.”  This  assumption  is  not 
correct  in  the  context  of  their  definition  of  “smooth  trajectories”.  Smooth  object 
motion  can  and  often  does  result  in  sharp  cusps  in  image  plane  trajectories  (see 
Figure  2.3  of  this  report)  which  violates  their  smoothness  definition.  If  feature  point 
trajectories  are  modelled  with  parametric  equations  in  time  {u{t),v{t)),  “smooth” 
three-dimensional  object  motion  will  result  in  u{t)  and  v{t)  being  “smooth”  functions 
of  time.  However,  if  the  image  plane  trajectory  is  written  as  w(<)  =  /[u(01» 
very  restrictive  to  assume,  as  in  [56],  that  /[•]  is  smooth.  A  possible  solution  to  the 
temporal  correspondence  problem  may  be  provided  by  sequential  state  estimation 
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Figure  1.1 

Hierarchical  structure  for  motion  analysis. 
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techniques,  such  as  the  Kalman  filter.  This  approach  hais  received  a  great  deal  of 
attention  in  radar-based  multi-target  tracking  literature  in  which  adaptive  Kalman 
filtering  or  hypothesis  testing  methods  are  proposed  to  track  multiple  manoeuvring 
targets.  Again,  feedback  from  motion  and  structure  components  of  higher  levels  of  the 
system  may  guide  the  two-dimensional  tracking,  or  temporaJ  correspondence  problem. 

Once  feature  positions  and  tem(>oraI  correspondence  have  been  established  for  im¬ 
age  sequences  of  each  camera,  higher  leveb  of  the  system  attempt  to  fuse  information 
from  multiple  cameras.  Level  V  focuses  on  three-dimensional  feature  tracking  and 
segmentation  of  feature  sets  into  groups  such  that  each  group  corresponds  to  a  rigid 
object.  The  process  of  matching  features  seen  in  two  or  more  images  taken  at  the 
same  time  instant  will  be  referred  to  as  ‘‘spatial  correspondence.”  Three  dimensional 
information  can  be  inferred  from  image  plane  feature  position  measurements  and  spa¬ 
tial  correspondence  in  two  or  more  cameras.  As  a  result,  this  processing  stage  leads 
to  a  three-dimensional  multi-target  tracking  problem. 

Initial  spatial  correspondences  must  be  established  through  epipolar  geometry 
together  with  any  information  available  from  two-dimensional  tracking  problems  of 
Level  IV.  Spatial  correspondence  for  binocular  stereo  pairs  is  simplified  if  the  image 
planes  are  parallel  and  physically  close  together.  In  this  case  corresponding  points 
have  almost  the  same  locations  and  possibly  similu  appearances  in  the  respective 
images.  However,  triangulation  then  becomes  ill-conditioned  and  highly  sensitive  to 
measurement  noise  due  to  the  nearly  parallel  three  dimensional  rays  representing 
inverse  perspective  projections  of  the  corresponding  points.  Spatial  correspondence 
is  simplified  and  more  robust  if  more  than  two  cameras  are  employed  (41). 

With  image  plane  feature  position  measurements,  and  temporal  and  spatial  cor¬ 
respondence,  sequential  state  estimation,  possibly  in  the  form  of  adaptive  Kalman 
filtering  with  hypothesis  testing,  may  provide  a  means  to  track  features  in  three 
dimensions.  Three  dimensional  position  and  velocity  information  will  be  useful  to 
segment  all  available  features  into  rigid  objects.  For  example,  in  the  case  of  feature 
points,  if  vectors  joining  any  two  three-dimensional  points  have  constant  length  (in 
a  statistical  sense,  i.e.  through  hypothesis  testing),  then  associate  these  two  points 
with  a  single  rigid  object.  It  may  also  be  i>ossibIe  to  incorporate  optical  flow  informa¬ 
tion  into  the  rigid  object  segmentation  component.  As  before,  feedback  from  higher 
levels,  particularly  the  object  tracking  component  of  Level  VI,  may  aid  in  the  spatial 
correspondence  and  rigid  object  segmentation  problems. 

Level  VI  comprises  motion  and  structure  estimation  or  three-dimensional  object 
tracking  and  is  the  major  focus  of  this  report.  Once  at  least  three  noncollinear  fea¬ 
ture  points,  for  example,  are  av^lable  for  a  single  rigid  object  from  Level  V,  three 
dimensional  object  state  estimation  may  be  initiated  in  order  to  further  refine  repre¬ 
sentations  of  the  observed  process.  For  the  purposes  of  this  investigation,  solutions  of 
the  important  problems  of  Levels  1  through  V  are  assumed;  the  cameras  are  assumed 
to  be  stationary  (although  methods  considered  herein  may  immediately  be  extended 
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to  the  case  of  moving  sensors),  noisy  feature  position  measurements  as  well  as  tem¬ 
poral  and  spatial  correspondence  information  are  assumed  to  be  available,  and  only  a 
single  rigid  object  is  present  in  the  multiple-camera  image  sequences.  These  assump¬ 
tions  are  very  common  in  simulation  studies  as  outlined  in  the  literature  review  of 
Section  1.1. 

Two  additional  levels  in  Figure  1.1  include  knowledge  refinement  in  Level  VII  and 
environmental  interaction  in  Level  VIII.  Knowledge  refinement  refers  to  the  accumu¬ 
lation  and  refinement  of  information  from  all  previous  levels  in  order  to  construct 
a  meaningful  representation  of  the  observed  processes.  This  decision-making  level 
might  be  responsible,  for  example,  for  filter  decoupling  once  structure  estimates  of 
rigid  objects  reach  a  pre-defined  confidence  level,  fusion  of  rigidly  attached  objects, 
maintaining  an  estimate  of  relative  motion  or  interaction  of  multiple  objects,  or  per¬ 
haps  object  recognition  and/or  classification.  The  primary  objective  of  most  vision 
systems  ultimately  is  to  interact  in  some  way  with  the  environment  or  at  least  to 
form  intelligent  decisions  concerning  all  observed  processes.  This  is  the  function  as¬ 
signed  to  Level  VIII  and  may  include,  for  example,  direct  control  of  dynamic  systems, 
path  planning  and  navigation,  threat  detection  with  assessment  and  evasive  strategy 
planning  (collision  avoidance),  or  perhaps  compilation  of  surveillance  information  in 
security  systems.  Knowledge  based  systems  will  no  doubt  play  a  major  role  in  forming 
solutions  to  these  higher-level  problems. 

The  hierarchical  structure  proposed  in  Figure  1.1  provides  a  means  to  proceed 
to  higher  lev-ls  once  sufficient  information  is  available,  or  revert  to  lower  levels  if 
insufficient  information  is  present  to  proceed  at  the  given  level,  particularly  at  Level 
VI.  Generally,  the  proposed  system  is  composed  of  three  coupled  multi-target  tracking 
systems  with  increasing  levels  of  complexity:  in  Level  IV  multiple  features  are  tracked 
in  the  image  planes;  Level  V  fuses  information  from  Level  IV  in  order  to  track  multiple 
features  in  three-dimensions;  auid  Level  VI  is  concerned  with  tracking  multiple  objects, 
composed  of  multiple  features  from  Level  V,  in  three  dimensions  with  six  degrees-of- 
freedom.  It  might  be  expected,  therefore,  that  statistical-based  multi-target  tracking 
methods,  which  have  been  extensively  studied  in  radar-based  tracking  applications, 
will  be  extremely  useful  in  forming  solutions  to  the  vision-based  motion  analysis 
problem. 

Another  problem  which  has  not  been  mentioned  above,  but  is  of  fundamental 
importance  in  feature-based  techniques,  is  that  of  feature  occlusion  due  to  object  mo¬ 
tion  or  other  objects  in  the  foreground  of  a  scene.  A  common  approach  in  simulation 
studies  is  to  assume  that  occlusion  does  not  occur  [7].  However,  recursive  approaches, 
such  as  the  Kalman  filter,  may  provide  natural  solutions  for  state  estimation  in  the 
presence  of  occlusion  since  measurements  need  not  be  of  the  same  set  of  features  at 
each  observation  event.  Feature  occlusion  is  an  important  component  of  the  tempo¬ 
ral  and  spatial  correspondence  problems  and  will  not  be  explicitly  examined  in  this 
report.  In  previous  work  [58]  which  is  reviewed  in  Chapter  2,  feature  occlusion  was 
treated  by  simply  proceeding  with  state  estimation,  but  in  the  absence  of  data  from 
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occluded  points. 

Evaluation  of  methods  for  Level  VI  on  real  imagery  is  beyond  the  scope  of  current 
work.  Such  an  a^ialysis  will  require  an  intense  review  of  feature  detection  and  corre¬ 
spondence  problems,  sensor  modeling  and  measurement  noise  characterization,  and 
complex  experimental  ;jTangements  in  order  to  obtain  long  sequences  (100  frames,  for 
example)  of  digital,  time-aligned,  recorded  imagery  and  ground  truth  data  associated 
with  object  and/or  camera  motion.  More  complete  implementations  and  performance 
anedysis  of  proposed  methods  with  real  imagery  constitute  major  proposals  for  future 
research  outlined  in  Chapter  5.  In  this  investigation,  suitable  imagery  with  or  with¬ 
out  occlusion  is  computer-generated  for  a  translating  and  rotating  block.  Features 
of  interest  are  the  block  corners  whose  positions  in  each  image  are  perturbed  by 
an  additive  pseudo-random  noise  process  and  made  available  to  structure  and  mo¬ 
tion  estimation  algorithms  along  with  correspondence  information.  This  approach  to 
simulation  analysis  is  consistent  with  similar  studies  in  the  literature  [15]  [28]-[34]. 


1.3  Chapter  Summary 

This  chapter  introduced  and  compared  image  flow  and  feature  based  approaches  to 
motion  analysis,  and  presented  a  survey  of  major  contributions  which  apply  extended 
Kalman  filtering  to  motion  and  structure  estimation.  A  possible  hierarchical  approach 
to  feature-based  motion  analysis  has  been  proposed  in  which  motion  and  structure 
estimation  represents  only  a  single  component.  Major  assumptions  employed  in  this 
investigation  that  serve  to  isolate  the  motion  and  structure  estimation  problem  have 
been  discussed.  The  remainder  of  this  report  is  organized  as  follows: 

Chapter  2  treats  an  important  but  simplified  problem  in  which  the  object  of  interest 
is  constrained  to  lie  on  a  known  planar  surface; 

Chapter  3  treats  the  more  general  object  tracking  problem  by  introducing  geom¬ 
etry  and  notation,  developing  state  space  models  for  three  parameterizations 
of  rotational  motion,  and  presenting  a  simple  single-frame  filter  initialization 
scheme; 

Chapter  4  compares  Cramer  Rao  bounds  of  the  three  parameterizations  for  a  sim¬ 
plified  trajectory,  and  presents  simulation  results  and  comparisons  of  estimation 
performance  of  the  three  filters  for  manoeuvring  object  trajectories; 

Chapter  3  provides  general  discussions,  conclusions,  and  proposals  for  future  work. 

Several  appendices  are  provided  so  that  this  report  may  be  self  contained: 

Appendix  A  provides  a  tutorial  on  parameterizations  of  relative  orientation  includ¬ 
ing  the  roll-pitch-yaw,  Euler  angle-axis,  and  quaternion  representations,  and 
equations  which  govern  the  temporal  behavior  of  parameters; 
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Appendix  B  gives  a  review  of  Kalman  and  extended  Kalman  filtering  for  continuous- 
and  discrete-time  systems,  as  well  as  modifications  such  as  local  iterations; 

Appendix  C  derives  closed-form  as  well  as  recursive  expressions  representing  the 
Cramer  Rao  lower  bounds  for  discrete-time  nonlinear  state  estimation; 

Appendix  D  lists  equations  of  measurement  model  Jacobians,  which  are  required 
in  the  extended  Kalman  filter  implementation,  for  the  three  parameterizations 
of  rotational  motion. 
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2.  Motion  On  A  Known  Planar 
Surface 


In  a  large  number  of  motion  analysis  problems,  particularly  in  many  robotics  ap¬ 
plications,  objects  of  interest  axe  constrained  to  lie  on  a  surface  which  is  known  or  can 
be  approximated  analytically.  This  chapter  briefly  introduces  the  Kalman  filtering 
approach  by  reviewing  previous  research  by  the  author,  [58,  59).  This  work  proposed 
linear  motion  and  structural  models  for  a  rigid  object  which  is  constrained  to  lie  on 
a  known  planar  surface,  and  a  nonlinear  measurement  model  which  describes  noisy 
monocular  (single  camera)  perspective  projection  observations  from  an  arbitrary  po¬ 
sition  above  the  known  plane.  As  in  the  work  of  Broida  et  al.  [32],  object  motion 
was  assumed  to  be  of  constant  translational  and  rotational  velocity.  These  assump¬ 
tions,  although  restrictive,  provided  a  simplified  framework  in  which  to  investigate 
the  performance  of  Kalman  filtering  methods  in  motion  analysis  problems. 

This  chapter  first  introduces  notation  and  geometry  for  reference  frames,  coor¬ 
dinate  transformations,  and  the  perspective  projection  image  formation  model  in 
Section  2.1.  Section  2.2  briefly  reviews  the  planar  motion  method  by  developing 
structure,  motion,  and  measurement  models,  and  presenting  a  reduced  set  of  simu¬ 
lation  results.  Section  2.3  summarizes  and  discusses  the  methods  and  results  of  this 
chapter. 


2.1  Notation,  Geometry  and 
Perspective  Projection 


Planar  motion  and  object  structure  are  recovered  from  positions  of  object  feature 
points  extracted  from  a  monocular  image  sequence.  Three  reference  frames,  denoted 


Fo  ~  Object-fixed, 

Fe  Earth-fixed,  and  (2.1) 

Fc  ~  Camera-fixed 


are  used  in  this  problem. 

A  reference  frame.  Fa,  where  o  €  {O,  E,C},  is  defined  by  a  point  Oa,  the  origin 
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of  Fa,  and  three  orthonormal  vectors  i^,  jo)  and  ko  which  lie  along  the  Xq,  ya,  and 
Za  axis  of  F.,  respectively.  A  vector  r  which  describes  the  position  of  a  feature  point 
on  the  object  with  respect  to  Oa  and  is  expressed  in  Fa  is  denoted  by  Ta-  The 
components  of  Ta  are  written  as  *‘c»  —  •'ffl.sla* 

A  second  reference  frame,  Fp,  is  related  to  Fa  by  a  translation  T^,  which  represents 
a  vector  from  Oa  to  Op  and,  when  expressed  with  respect  to  Fa,  is  written  as  [T^]o, 
and  an  orthogonal  3x3  matrix,  I^,  which  denotes  a  transformation  from  the  a-basis 
to  the  )9-basis  and  represents  a  rigid  rotation  of  IR^.  Appendix  A  provides  a  tutorial 
review  of  the  Euler  angle-axis,  quaternion,  and  roll-pitch-yaw  parameterizations  for 
basis  transformations  represented  by  The  planar  method  employs  only  the  yaw 
angle  of  the  roll-pitch-yaw  parameterization.  With  this  notation, 

[^alp  =  I^r^,  and  (2.2) 

'fi  =  (2.3) 

If  Optical  distortions  of  an  imaging  device  are  not  severe,  a  perspective  projection 
image  formation  model  can  be  used  to  relate  the  observed  location  of  a  feature  in 
a  two-dimensional  image  to  the  three-dimensional  coordinates  of  that  feature  in  a 
camera-centred  reference  frame.  Figure  2.1  shows  the  assumed  geometry  for  the 
image  formation  model.  The  position  of  the  ith  feature  point,  p*,  with  respect  to  a 
camera-centred  reference  frame,  Fc,  is  described  by  the  vector  r’^.  A  feature  at  point 
p*  is  projected  onto  the  image  plane  and  appears  at  point  {L,  u',  v')  in  Fc,  where  L  is 
the  effective  focal  length.  The  origin  of  Fc  is  taken  at  the  centre  of  projection  of  the 
imaging  lens.  The  xc  axis  of  Fc  is  aligned  with  the  optical  axis,  while  yc  is  parallel  to 
a  scan  line  and  positive  to  the  right  in  the  image.  The  Zc  axis  is  positive  down  in  the 
image  to  form  a  right-handed  coordinate  system.  Neglecting  separate  horizontal  and 
vertical  scale  factors,  and  assuming  any  distortion  effects  are  negligible,  the  projected 
image  plane  position  of  feature  point  p*  is  given  by 


The  central  projection  transformation  defined  in  (2.4)  demonstrates  the  loss  of  depth 
information  in  monocular  imaging  systems;  the  image  position  (u',v*)  is  invariant 
under  any  scalar  multiple  of  r^. 

Figure  2.2  shows  a  typical  physical  arrangement  for  the  planar  motion  problem. 
The  object  of  interest,  shown  here  as  a  block,  is  described  by  feature  points,  for 
example  the  block  corners,  in  Fq.  The  earth-fixed  reference  frame,  Fe,  is  defined 
such  that  the  xe-Ve  coordinate  plane  is  the  plane  of  motion.  The  xo-yo  plane  of 
Fq  is  defined  to  be  parallel  to  the  plane  of  motion  in  which  case  the  axis  of  rotation 
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Figure  2.1 

Central  projection  image  formation  geometry.  A  feature  at  point  p* 
projects  onto  the  point  in  the  camera-centred  frame,  Fc, 

where  L  is  the  effective  focal  length. 

is  parallel  to  Z£.  The  position  and  orientation  of  Fc  is  assumed  to  be  known  with 
respect  to  Fe- 


2.2  Estimation  of  Structure  and  Planar  Motion 

The  extended  Kalman  filtering  (EKF)  approach  requires  first  that  dynamical  and 
measurement  models  be  expressed  in  state  space  form.  Appendix  B  provides  a  review 
of  the  filtering  equations  which,  in  general,  are  based  on  a  dynamical  (plant  or  process) 
model  of  the  form 

x(t)  =  f[x(t)]  +  G{f)w(f),  (2.5) 

where  the  white  noise  process*  w(<)  ~  N{0,Q{t))  is  mapped  to  the  state  space 
through  the  matrix  G.  The  measurement  model  is  expressed  in  discrete-time  form, 
since  images  arrive  at  discrete  instants  in  time,  as 

z(k)  =  h(x(<fc)]  +  v(k),  (2.6) 

where  the  measurement  noise  process,  v(ib)  ~  iV(0,R(A:)),  is  also  assumed  to  be 
temporally  white.  Motion,  structure,  and  measurement  models  for  the  planar  motion 
problem  are  required  in  the  above  form. 

*Tbe  notation  N(m,  C)  denotea  a  jointly  Gauaaian  population  with  mean  vector  m  and  covariance 
matrix  C. 


DRES-SR-577 


18 


Figure  2.2 

System  geometry  for  planar  motion  analysis. 

Motion  and  Structure  Models:  The  position  of  the  origin  of  Fo  relative  to 
Fe  is  represented  by  [7o']£;(<)  =  The  orientation  of  Fq  relative  to  Fe 

is  described  by  the  yaw  angle  V’CO  whose  (constant)  time  derivative  w  defines  the 
rotational  velocity  vector  as  w  =  [0,0,  w]£.  The  state  vector  for  object  motion  is 
then  given  by 

Xm(0  =  [*(0.  y(0»  y,  ^(0.  .  (2-7) 

where  superscript  ‘7”  denotes  the  matrix  transp>ose. 

The  origin  of  Fq  is  defined  to  have  constant  translational  velocity  and  lies  at  the 
intersection  of  the  rotational  axis  and  the  plane  of  motion.  Note  in  this  case  that 
the  object-centred  frame  depends  on  the  assumed  nature  of  object  motion,  which 
results  in  unobservability  in  the  absence  of  rotational  motion.  Further  comments  on 
this  problem  are  given  in  Section  3.2.  It  is  also  assumed  that  one  feature  point  is 
observed  whose  height  above  the  plane  of  motion  is  known  or  estimated.  This  point 
is  fixed  in  the  (vertical)  Xo-zo  plane.  Hence  both  the  Zo-  and  yo-coordinates  of  this 
special  feature  point  are  known  and  removed  from  the  estimation  process.  In  order 
to  simplify  notation  in  the  following  development,  removal  of  two  coordinates  of  one 
feature  point  is  neglected. 
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The  full  state  vector  which  describes  object  motion  and  structure  is  defined  as 


x(0  = 


(2.8) 


where  x,  is  the  structure  state  vector, 

X.  =  (2-9) 

and  Nj  is  the  number  of  feature  points  of  interest.  A  linear  discrete-time  difference 
equation  which  describes  time  propagation  of  the  state  x{t)  over  a  sample  period 
can  be  written  as 

x(Jb  -I- 1)  =  ♦(jk)x(ib)  +  G(ib)w(ib),  (2.10) 

where 

*(*)  = 

in  which  Op,,  represents  &  p  x  q  block  of  zeros,  Ip  is  the  p  xp  identity  matrix,  and 


^m(k) 

OsNyxS  IsN/ 


(2.11) 


♦m(lk)  = 


1  Tfc  0  0  0  0  ■ 

0  1  0  0  0  0 

0  0  1  Tfc  0  0 

0  0  0  1  0  0 

0  0  0  0  1  r* 

0  0  0  0  0  1 


(2.12) 


Higher-order  motion  may  be  incorporated  into  the  system  model  which  remains  linear, 
due  to  the  planar  motion  assumption,  in  the  elements  of  the  state  vector.  The  overall 
state  transition  matrix  9{k)  in  (2.11)  is  extremely  sparse  which  leads  to  very  efficient 
time  propagation  in  the  discrete-time  extended  Kalman  filter  implementation. 

Measurement  Model:  The  instantaneous  position  and  orientation  of  Fq  relative 
to  Fe  are  given  by  the  translation 

[Ti]^«)  =  |x(l).»(0,0g,  (2.13) 

and  basis  transformation  matrix 


ti:W  = 


COS  0 

sin0 

0 


—  sin  rj;  0 
0 

0  1 


(2,14) 


while  the  known  position  and  orientation  of  Fc  relative  to  Fe  are  defined  by  and 
I§,  respectively.  The  position  of  the  ith  feature  point  with  respect  to  Fc  is  written 


=  (2.15) 
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which  can  be  expressed  in  component  form  (see  Appendix  A)  as 
’’c.i  =  +  [Tq]^  -  [Tc]^}  » 

rb  =  [icll{lgri,+  lTg]^-|T§]^},  (2.16) 

rb  =  [ME{l2''b+ [To]j- [Tf]J. 


With  Nj  feature  points  of  interest  the  measurement  model  is  given  by 


»(*)  =  +  V(l!), 


(2.17) 


with 


h[x(A:)l  = 


hMxWi 

h^xik)] 


[  h^/lx(fc)]  J 


(2.18) 


where  the  image  plane  position,  h’[x(fc)],  of  the  tth  feature  point  is  computed  with 
the  perspective  projection  transformation  of  (2.4), 


h‘[x(t)) 


M(x(*)]  ‘ 

—  r 

*■0.1 

M{x(*)l  J 

ij 

.  >•0.1  . 

(2.19) 


and  v(A;)  models  the  measurement  noise  which  is  assumed  to  a  discrete-time,  zero- 
mean,  white  Gaussian  process  with  covariance  matrix  R(ik). 

A  single  image  provides  2Nf  equations  in  6-f  3A//  —  2  unknowns  (since  one  feature 
point  has  two  known  coordinates).  Each  additional  feature  point  provides  two  more 
equations,  but  increases  the  dimension  of  the  state  vector  by  three  unknowns.  With 
four  feature  points,  for  example,  each  image  provides  8  equations  in  16  unknown 
vwables.  In  this  case,  at  least  two  image  frames  are  required  to  obtain  more  equations 
than  unknowns.  Three  of  these  variables  (z,  y,  are  time  varying  while  all  others 
are  constant. 

Simulation  Results:  This  discrete-time  state  estimation  problem  is  formulated 
with  a  linear  dynamic  model  and  a  nonlinear  measurement  model.  The  discrete¬ 
time  extended  Kdman  filter  (EKF)  was  found  to  yield  satisfactory  results  without 
local  iterations  when  object  structure  is  known,  but  demonstrates  somewhat  slower 
convergence  in  the  absence  of  prior  structural  information.  The  iterated,  extended 
Kalman  filter  (  lEKF),  which  is  also  reviewed  in  Appendix  B,  generally  gives  signifi¬ 
cant  improvements  in  the  rate  of  convergence  while  structure  is  being  estimated.  In 
extensive  simulations  [58],  the  lEKF  demonstrated  reliable  convergence  to  true  pa¬ 
rameters  with  a  simple  single-frame  initialization  of  object  position  and  orientation, 
and  with  all  other  parameters  set  to  zero.  This  Section,  however,  shows  Monte  Carlo 
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simulation  results  based  ou  60  runs  with  random  filter  initialization  taken  from  a 
Gaussian  population  centred  about  the  true  initial  state,  i.e.,  simulations  assume  the 
availability  of  an  unbiased  estimator  of  x{0)  with  statistics  x©  ~  N(x(0),Po),  where 
the  covariance  matrix  P©  defines  the  level  of  prior  information  in  the  initial  estimate 
X©.  Filter  initialization  is  then  taken  as  x(0|  —  1)  =  x©  and  P(0l  —  1)  =  P©.  Filter 
mean  errors  (ME)  and  root-mean-square-errors  (RMSE)  are  computed  immediately 
following  each  observation  event  over  the  60  simulation  runs. 

The  EKF  is  generally  a  nonlinear,  suboptimal  state  estimation  technique.  A 
powerful  result  which  provides  a  performance  assessment  of  parameter  estimation 
techniques  is  the  Cramer- Rao  inequality  [79].  Appendix  C  reviews  the  significance 
and  derivation  of  Cramer-Rao  lower  bounds  (CRLB)  for  this  estimation  problem. 
Generally,  the  error  covariance  matrix  S{N)  of  an  unbiased  estimator  x(A^)  of  the 
state  x(iV),  where  the  estimate  is  based  on  the  prior  information  in  x©  and  the 
observations  z(0), . . . ,  z(A^),  is  lower-bounded  by  the  inverse  of  Fisher’s  information 
matrix  [80,  pp.  91-93).  This  result,  in  theory,  provides  estimation  error  variance 
lower  bounds  for  elements  of  x(A^).  It  should  be  noted,  however,  that  these  bounds 
assume  an  unbiased  estimation  procedure;  a  state  dependent  bias  can  result  in  higher 
or  lower  bounds  [78]  (see  Appendix  C).  As  a  result,  most  analysis  [28]-[31],  [61], 
[62],  proceed  under  the  assumption  of  unbiased  estimation  resulting  in  approximate 
covariance  bounds. 

The  geometry  of  Figure  2.2  was  used  to  generate  sequences  of  image  feature  points 
with  occlusion  and  correspondence  information.  The  camera  position  and  orientation 
is  defined  by  a  translation  [T^]^  =  [0,  —30,  —30]^,  and  the  transformation  formed 
from  Euler  angles  ipe  =  45®  (yaw  or  pan),  9c  =  —45®  (pitch  or  tilt),  and  =  0  (roll 
about  optical  axis). 

The  object  is  described  by  eight  corners  of  a  solid  rectangular  block  20  units  wide 
and  10  units  in  height  and  depth.  Four  feature  points  labelled  p*,  p?,  p*,  and  p^  define 
the  structure  of  the  block.  It  was  found  that  Cramer-Rao  bounds  do  not  decrease 
significantly  when  more  than  four  feature  points  are  considered.  Points  p^  and  p^  are 
diagonally  opposite  corners  on  the  bottom  face  of  the  block,  and  hence  are  subject  to 
occlusion  due  to  object  motion.  Points  p^  auid  p^  au’e  diagonally  opposite  comers  on 
the  top  face  of  the  block  and  are  diagonally  opposite  to  p’  or  p*  on  side  faces.  Point 
p^  (sometimes  occluded)  is  selected  as  the  special  feature  point  which  is  known  to 
lie  in  the  plane  of  motion  and  is  further  assumed  to  lie  on  the  negative  Xo  axis.  All 
structure  parameters  are  retained  in  the  state  vector  during  occlusion. 

The  true  trajectory  is  defined  by  an  initial  position  (z(0),y(0))  =  (30,  —25)  units, 
orientation  ^(0)  =  2.1  radians,  constant  translational  velocity  (i,y)  =  (2.5, 5.0) 
units/s,  and  constant  angular  velocity  w  =  —0.5  radians/s.  Initial  estimate  uncer¬ 
tainty  is  defined  by  a  diagonal  P©  in  which  the  square  roots  of  the  diagonal  entries 
represent  standard  deviations  of  5  units  for  position,  3  units/s  for  translational  veloc¬ 
ity,  0.35  radians  for  angular  position,  1.0  radians/s  for  angular  velocity,  and  5  units 
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for  each  of  the  10  structural  parameters  (the  y-  and  ^-coordinates  of  p*  are  not  esti¬ 
mated).  Local  iterations  at  each  observation  event  are  employed  with  a  maximum  of 
10  and  terminating  if  the  maximum  element  of  the  difference  in  successive  iterates  is 
less  than  0.01.  In  most  cases,  only  5  to  7  iterations  were  required  during  initial  ob¬ 
servation  events,  but  as  the  filter  accumulates  information,  only  one  or  two  iterations 
were  necessary. 

Figure  2.3  shows  a  true  and  a  sample  noisy  image  sequence  of  feature  point  tra¬ 
jectories  with  additive  measurement  noise  of  standard  deviation  =  0.02  units  and 
unity  focal  length.  A  20  second  time  interval  is  employed  with  a  sample  period  of 
T  =  0.1  seconds.  The  measurement  noise  level  of  0.02  units  represents  quantities  on 
the  order  of  5%  to  10%  of  the  object  image  size  when  the  object  is  closest  to  the 
camera,  but  as  the  object  moves  away  from  the  camera  noise  levels  are  on  the  order 
of  11%  to  23%  of  the  object  image  size  at  the  end  of  the  trajectory.  A  feature  point 
occlusion  map  is  shown  in  Figure  2.4.  Feature  point  p*  is  visible  in  the  first  9  image 
samples  or  almost  1  second  before  being  occluded  for  about  3  seconds.  The  special 
feature  point  p®  is  occluded  during  time  periods  from  6.5  to  10  seconds  and  from 
about  19  seconds  to  the  end  of  the  trajectory. 

Figures  2.5  and  2.6  show  the  root-mean-squared  error  (RMSE),  Cramer- Rao  lower 
bound  (CRLB),  and  mean  error  (ME)  results  for  a  60  run  Monte  Carlo  simulation. 
Figure  2.5  shows  results  for  motion  parameter  estimation.  Position  and  orientation 
estimates  tend  to  be  biased  over  the  first  10  seconds  as  indicated  by  the  ME  trace. 
After  10  seconds,  the  RMSE  for  z-position  approaches  and  follows  the  CRLB.  RMS 
errors  for  y-position  and  velocity  are  also  initially  biased,  but  approach  and  follow 
the  CRLB  just  after  4  seconds.  Angular  position  RMSE  remuns  above  the  CRLB 
throughout  the  trajectory  and  tends  to  increase  slightly  during  the  occlusion  of  p®, 
but  demonstrates  only  small  bias  after  10  seconds.  Good  performance  is  shown  in 
the  estimation  of  angular  velocity  and  z- velocity. 

Figure  2.6  shows  results  for  structure  estimation  of  one  point,  p^,  on  the  top  of 
the  block  (not  subject  to  occlusion),  and  both  points,  p*  and  p®,  on  the  bottom  of 
the  block.  Satisfactory  RMSE  performance  is  demonstrated  for  p^  structure  after  5 
seconds  with  negligible  bias.  Simulation  results  for  estimation  of  p'  structure  (not 
shown)  are  almost  identical  to  those  shown  for  fP.  Errors  in  estimates  of  p^  structure 
during  occlusion  may  be  largely  responsible  for  bias  in  other  state  estimates  over  the 
initial  portion  of  the  trajectory.  The  presence  of  p^  in  images  sampled  after  1  =  4 
seconds  results  in  a  rapid  decrease  in  structure  RMS  errors  as  estimation  performance 
approaches  the  CRLB.  Estimation  of  the  zo-coordinate  of  p®  is  also  somewhat  biased 
particularly  just  after  p^  becomes  visible  and  during  the  time  interval  6  <  t  <  10 
seconds  when  p®  itself  is  occluded.  RMS  errors  of  all  structural  parameters  approach 
and  follow  the  CRLB  towards  the  end  of  the  trajectory. 
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Figure  2.5 


Planar  motion  parameter  Monte  Carlo  simulation  results.  All  plots 
show - RMSE, - CRLB,  and . ME. 


DRES-SR-577 


26 


UNCLASSIFIED 


Figure  2.6 

Planar  motion,  structure  parameter  Monte  Carlo  simulation  results. 
All  plots  show - RMSE, - CRLB,  and . ME. 
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2.3  Chapter  Summary 

This  chapter  has  introduced  notatioii  for  reference  frames  and  coordinate  trans¬ 
formations  that  will  be  used  extensively  in  following  chapters  and  in  the  Appen¬ 
dices.  The  perspective  projection  image  formation  model  is  fairly  standard  in  image 
analysis  work,  with  the  exception  of  some  methods  which  are  based  on  orthographic 
projection — a  simplification  which  leads  to  mathematically  tractable  systems  of  equa¬ 
tions  in  some  cases.  Only  perspective  projections  will  be  considered  in  this  work. 

Previous  research  proposed  motion,  structure  amd  measurement  models  which  de¬ 
scribe  motion  of  a  rigid  object  constrained  to  a  known  planar  surface  with  monocular 
observations  from  above  the  known  plane.  Monte  Carlo  simulation  results  compared 
to  approximate  Crauner-Rao  bounds  demonstrated  good  performance  in  recovery  of 
absolute  motion  and  structure  through  iterated  extended  Kalman  filtering  which  par¬ 
allels  the  approach  of  Broida  et  al.  [32].  This  investigation  demonstrated  filter  per¬ 
formance  that  might  be  expected  for  constant  translational  and  rotational  velocity. 
A  fundamental  assumption  of  this  technique  is  that  the  height  of  one  feature  point 
above  plane  is  known  or  approximated.  Another  important  limitation  of  the  proposed 
technique  and  methods  of  Broida  et  al.  [32]  and  Young  et  al.  [35]  is  that  the  systems 
as  formulated  are  not  observable  for  zero  angular  velocity.  The  absence  of  rotational 
motion  must  be  detected  during  initialization,  and  an  alternate  parameterization  is 
required  in  which  one  feature  point  is  selected  as  the  origin  of  the  object-fixed  frame 
and  angular  velocity  is  removed  from  the  state  vector.  Filter  performance,  in  fact, 
was  found  to  be  seriously  degraded  as  rotational  rates  become  very  small. 

Current  research,  which  is  the  subject  of  subsequent  chapters  of  this  report,  re¬ 
moves  assumptions  regarding  partial  knowledge  of  the  position  of  some  special  feature 
point,  extends  motion  models  to  six-degree-of-freedom  manoeuvring  object  trajecto¬ 
ries,  extends  the  measurement  model  to  multiple  imaging  systems,  and  investigates 
three  parameterizations  of  rotation^  motion.  In  addition,  the  object-centred  frame 
is  defined  independent  of  object  motion  so  that  the  system  is  observable,  provided  at 
least  three  feature  points  are  available,  for  arbitrary  object  motion. 
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3.  Approximate  Models  for 
General  Motion 


Methods  for  planar  motion  considered  in  the  previous  chapter  employed  several  as¬ 
sumptions  which  may  be  difficult  to  satisfy  or  verify  in  analysis  of  general  scenes.  This 
chapter  develops  approximate  dynamic  models  for  six-degree-of-freedom  manoeuvring 
object  motion,  and  a  perspective  projection  measurement  model  for  multiple-camera 
imaging  systems.  As  in  the  planar  motion  problem,  dynamical  and  measurement 
models  must  be  obtained  in  state  space  form  as 


x{t)  =  f(x{0]  +  G(0w(<),  (3.1) 

z(fc)  =  h[x(4)]  +  v(fc),  (3.2) 


where  the  process  noise  w(<)  ~  A(0,Q(f)),  which  is  mapped  to  the  state  space 
through  the  matrix  G,  and  the  measurement  noise,  v(fc)  ~  iV(0,R(l:)),  are  assumed 
to  be  temporally  white. 

Notation  and  assumed  geometry  are  introduced  in  Section  3.1.  Section  3.2  dis¬ 
cusses  the  structural  model  for  a  rigid  object.  The  measurement  model  for  observar 
tions  with  multiple  imaging  systems  is  presented  in  Section  3.3.  Translational  motion 
models  are  developed  in  Section  3.4,  and  approximate  dynamic  models  for  the  Euler 
angle-axis,  roll-pitch-yaw,  and  quaternion  parameterizations  of  rotational  motion  are 
proposed  in  Section  3.5.  A  simple  initialization  scheme  based  on  a  single  measure¬ 
ment  event  is  the  subject  of  Section  3.6.  A  brief  chapter  summary  is  provided  in 
Section  3.7. 


3.1  Notation  and  Geometry 


This  chapter  employs  the  same  notation  for  reference  frames  and  coordinate  trans¬ 
formations  as  presented  in  Section  2.1,  but  with  multiple-camera  image  sequences. 
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Figure  3.1 

System  geometry  for  general  motion  analysis. 


multiple  camera  reference  frames  are  required.  Reference  frames  denoted  as 

Fo  Object-fixed, 

Fe  Earth-fixed,  and 

Fc,  ^  jth  Camera-fixed, y  =  1,2,..., JVc  '  ‘  * 

for  yVc-ocular  imaging  systems, 

are  used  in  the  following  development. 

Figure  3.1  illustrates  a  typical  physical  arrangement  for  the  problem  considered 
in  this  work.  Two  or  more  cameras  observe  a  single  rigid  object  whose  motion  is 
both  translational  and  rotational.  Image  formation  and  axis  orientation  for  each 
camera  frame  is  as  described  in  Figure  2.1  of  Section  2.1.  The  position, 
and  orientation,  of  the  yth  camera  frame,  Fc,,  j  =  1,2, . . . ,  is  known  or 
estimated  with  respect  to  the  stationary  earth  fixed  frame,  Fe-  Object  structure  is 
again  formulated  in  terms  of  positions,  r^,  of  feature  points  expressed  with  respect 
to  the  object-centred  frame,  Fq,  while  object  motion,  which  consists  of  translational 
motion  of  the  origin  of  Fq  together  with  rotational  motion  of  Fo,  is  described  with 
respect  to  the  stationary  earth-fixed  frame  Fe-  As  a  result,  parameterization  of 
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object  motion  and  structure  is  independent  of  position,  orientation,  and  motion  of 
the  imaging  systems. 

The  overall  state  vector  x  €  IR^  includes  states  corresponding  to  translational 
motion,  rotational  motion  and  object  structure: 


where 


X  = 


X,  ■ 
Xr  , 

.  . 


X|  6  Translational  motion  state  vector, 

x,€lR^'  Rotational  motion  state  vector, 

X,  €  ~  Structure  state  vector. 


The  dynamical  model  is  written  in  the  general  form 


[  fiM)]  1 

r  Gt  0  1 

■ 

x(<)  = 

fr[Xrit)] 

0 

+ 

1 - 

o  o 

of 
■ _ 

.  . 

(3.4) 


(3.5) 


(3.6) 


where  subscripts  ‘t’,  and  V'  denote  translational  and  rotational  motion  models,  re¬ 
spectively.  As  indicated,  elements  of  the  structure  state  vector  are  constant  in  time. 

3.2  Structure  Model 

The  rigid  object  is  defined  by  Nj  feature  points,  p*,  i  =  1,2, ...,Ar/,  whose 
position  vectors  with  respect  to  Fo,  rjj,  collectively  define  the  structure  set  5  = 
. . . ,r0^}.  Observation  of  at  least  three  noncollinear  feature  points  by  at 
least  two  cameras  at  each  observation  event  provides  sufficient  information  to  fix 
an  object-centred  frame  on  the  object.  A  number  of  techniques,  including  [28]- [38], 
[58,  59],  define  an  object-centred  frame  whose  orientation  and  position  with  respect 
to  the  object  itself  is  based  in  part  on  the  assumed  motion  of  the  object.  For  example, 
in  Chapter  2,  the  origin  of  the  object-centred  frame  is  defined  to  be  a  point  which 
satisfies  the  following  construnts; 

1.  Rigid  object  assumption  -  The  position  of  the  origin  with  respect  to  all  observed 
feature  points  is  constant  in  time; 

2.  Planar  motion  assumption  -  The  origin  lies  in  the  known  plane  of  motion;  and 

3.  Constant  velocity  assumption  -  The  origin  moves  with  constant  translational 
velocity  (on  the  axis  of  rotation)  with  respect  to  the  earth-fixed  frame. 
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Figure  3.2 

Fixing  the  object-centred  frame  with  three  feature  points. 

This  leads  to  unobservability  even  if  the  assumption  of  constant  translational  velocity 
is  correct  in  the  absence  of  rotational  motion  since  in  this  case  an  infinite  number  of 
points  will  satisfy  items  1-3  above.  Similar  arguments  can  also  be  applied  to  methods 
proposed  in  [28]-[38]  which  cannot  be  directly  employed  in  the  case  of  manoeuvring 
objects  for  which  the  true  nature  of  translational  and  rotational  motion  is  unknown. 
In  this  case,  the  object-centred  frame  must  be  independent  of  object  motion  and 
defined  on  the  basis  of  observed  feature  points  only. 

In  the  present  work,  Fq  is  fixed  on  the  object  by  selecting,  as  will  be  discussed 
further  in  Section  3.6,  three  noncollinear  feature  points  p®,  p®}  i  (three  noncollinear 
image  points  are  necessarily  projections  of  three  noncollinear  3D  points)  and  imposing 
the  following  constraints  as  demonstrated  in  Figure  3.2: 

1.  Feature  point  p*  lies  at  a  known  position  do  =  [do,i,do.a,do;il^  in  Fq,  and 
hence  its  structure  vector  is  known; 
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2.  One  of  the  coordinate  axis  of  Fo,  say  the  Xo  axis,  is  parallel  to  the  line  passing 
through  and  p^,  in  which  case  the  yo-  and  zo-coordinates  of  the  structure 
vector  of  p*  are  known  to  be  do.2  and  do,3»  respectively;  and 

3.  One  of  the  coordinate  planes  of  Fo,  say  the  Xo-yo  plane,  is  parallel  to  the  plane 
containing  p* ,  p*,  and  p^,  in  which  case  the  zo-coordinate  of  the  structure  vector 
of  the  third  point  is  known  to  be 

Selection  of  the  vector  do,  which  is  fixed  at  the  time  of  filter  initialization  and  there¬ 
after  remains  constant,  will  also  be  discussed  in  Section  3.6.  This  method  results  in 
one  of  four  possible  orientations  of  Fo  with  respect  to  the  three  points:  feature  point 
p^  in  item  2  could  have  a  positive  or  negative  Xo  coordinate,  and/or  feature  point 
j?  in  item  3  could  have  a  positive  or  negative  yo  coordinate.  These  four  discrete 
solutions  are  related  by  diagonal  basis  transformations  (rotations  of  the  basis  set  by 
0  or  T  about  a  coordinate  axis).  The  three  special  feature  points  would  most  likely 
be  selected,  as  outlined  in  Section  3.6,  from  a  larger  set  of  all  available  feature  points, 
which  leads  to  many  possible  positions  and  orientations  of  Fq  with  respect  to  the  ob¬ 
ject.  Once  the  initial  ordering  of  three  special  points  has  been  specified,  depending  on 
the  initialization  error  and  filter  transients,  any  one  of  the  four  possible  orientations 
of  Fo  is  possible.  Obviously,  the  estimation  process  is  likely  to  be  poorly  behaved 
if  images  of  these  three  points  are  very  close  together  relative  to  measurement  noise 
levels.  The  important  advantage,  however,  is  that  Fo  is  defined  independent  of  object 
motion. 

The  approach  taken  here  is  to  use  measurements  of  feature  positions  in  the  images 
directly,  and  enforce  the  three  constrsunts  above  by  fixing  the  six  known  structural 
coordinates  indicated  in  items  1,2,  and  3  by  excluding  these  parameters  from  the  esti¬ 
mation  process.  Explicit  removal  of  the  six  structural  coordinates  from  the  structure 
state  vector  in  the  development  of  dynamic  and  measurement  models  below  would 
lead  to  unnecessarily  complicated  notation.  For  brevity,  the  full  structure  state  vec¬ 
tor  is  indicated  by  the  notation,  but  it  is  understood  that  these  six  coordinates  are 
removed  from  the  estimation  process.  The  structure  state  vector,  x,  6  IR^*,  where 
N,  =  37V/  is  written  as 

(3.7) 

For  a  rigid  object,  the  elements  of  x«  are  constant  in  time. 

At  any  given  sample  time,  only  a  subset  of  feature  points  5v^  Q  S  will  be  visible 
in  the  image  of  the  jth  camera  due  to  occlusion  of  feature  points,  i.e.,  the  same  set 
of  feature  points  need  not  be  observed  by  all  cameras  at  each  measurement  event. 
In  addition,  the  total  number  of  feature  points,  N/,  is  generally  an  integer-valued 
function  of  time;  as  new  object  features  become  of  interest,  they  might  be  added 
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to  the  structure  set,  and  when  structure  estimates  reach  some  pre-defined  threshold 
confidence  level,  they  might  be  removed  from  the  structure  set  and  the  estimation 
process.  These  important  aspects  of  motion  analysis  are  not  explicitly  examined 
in  this  report.  In  the  present  work,  it  is  assumed  that  at  least  three  noncollinear 
feature  points  are  observed  by  at  least  two  cameras  at  each  observation  event,  and 
that  the  same  three  points  are  available  without  occlusion  over  the  simulation  time 
interval.  These  assumptions  should  be  viewed  as  simplifications  for  development  and 
presentation  of  the  problem  under  consideration  rather  that  restrictions  imposed  by 
the  proposed  methods. 

3.3  Measurement  Model 

The  measurement  model  is  formed  from  a  set  of  Nc  individual  measurement  mod¬ 
els  which  have  identical  form  and  each  corresponds  to  a  single  camera.  The  instan¬ 
taneous  position  and  orientation  of  Fq  relative  to  Fe  are  given  by  the  translation 

['*'ols(0  =  p(0  =  (i(<),»(').2(0£.  (3.8) 

and  basis  transformation  l£(C)i  respectively,  in  which  elements  of  the  vector  C  depend 
on  the  parameterization  of  rotational  motion.  The  known  instantaneous  position 
and  orientation  of  the  jth  camera  frame,  with  respect  to  Fe  is  specified  by  a 
translation  vector  and  basis  transformation  respectively.  The  position  of  the 
tth  feature  point  with  respect  to  Fc^  can  be  written  as 

rfc,=I?,{lSt'o  +  lTg]^-(TiJJ.  (3.9) 

and  writing 

Ic,  =  (IccJe]  ,  (3.10) 

gives  in  component  form  as 

=  [*C>] E  {^*‘o  +  ["^o]  E  ~  fi}  ’ 

’■c,.  =  N£{l?rj,+  [Tg]^-[TfJJ,  (3.11) 

=  [kc^l £  {ifirj,  -f-  [Xg] ^  -  [Xgj^} . 

With  Nj  observed  feature  points,  the  measurement  model  for  the  jth  camera  is 
given  by 

Zj{k)  =  h,{x(A:)]  -I-  (3.12) 

where 

hjlx(<:)l 

h)[x{k)] 

hi[x(*)]=  :  ,  (3.13) 

h,'''(x(Ar))  , 
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in  which  subscripts  refer  to  the  camera  and  superscripts  refer  to  the  feature  point. 
The  image  plane  position,  hj[x(ib)],  of  the  tth  feature  point  in  the  jth  camera  is 
computed  through  perspective  projection  as 


h;[x(*:)]  = 


hUi^k)] 


(3.14) 


The  Kalman  filtering  approach  assumes  that  measurement  noise,  Vj(t),  in  (3.12)  is 
a  discrete-time,  zero- mean,  white  Gaussian  process  with  known  covariance,  R>(k). 
The  proportionality  factor  Lj  in  (3.14)  represents  the  eflfective  focal  length  of  the  jth 
camera. 


The  overall  measurement  model  is  written  as 


where 


z(fc)  =  h[x(l:)]  -1-  v(fc). 


*1 

r  hi  1 

Vi 

z  = 

Zi 

h. 

,  and  V  = 

V2 

.  hNc  . 

(3.15) 


(3.16) 


Implementation  of  the  extended  Kalman  filter,  which  is  reviewed  in  Appendix  A, 
and  estimation  of  Cramer  Rao  bounds,  which  are  reviewed  in  Appendix  B,  requires 
the  computation  of  ,  l£(C)  ^md  h[x]  as  well  as  the  Jacobian  of  h[x], 

H[x|  S  (3.17) 

from  the  most  recent  state  estimate.  Computation  of  H[x]  is  a  straight-forward  but 
tedious  exercise  in  differentiation,  however,  with  algebraic  manipulation  and  simpli¬ 
fication,  H  can  be  written  in  simple  form  which  admits  computationally  efficient 
implementation.  Elquations  of  the  measurement  model  Jacobian  for  the  three  param- 
eterizations  of  rotational  motion  are  given  in  Appendix  D. 


3.4  Translational  Motion 

Translational  motion  in  object  tracking  problems  is  commonly  modelled  with  a 
truncated  Taylor  series  expansion  in  time.  In  this  case  the  position,  p(<)  =  [To]£(f), 
of  the  object-centred  frame  over  any  sample  period,  {<|  t*  <  t  <  L+i),  is  modelled 
with  an  {Np  —  l)th-order  polynomial  in  time, 

P(<)=  E  (*18) 

.=0 
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A  state  vector  corresponding  to  translational  motion,  X|  €  IR^*,  where  Nt  =  3Np,  is 
defined  as 


Xi  = 


[p  ’P‘’ 

which  propagates  in  time  according  to 


Xtii)  =  A<xt(t)  +  G,w,(<), 


where 


03(Wp-l)x3  I3(A;-1) 

,  and  G,  = 

03(Arp-l)x3 

03x3(A(^_1)  03x3 

(3.19) 

(3.20) 

(3.21) 


and  W|  G IR^  is  a  zero  mean,  temporally  white  Gaussian  process  with  covsiriance  Q(. 
In  this  case,  the  manoeuvre  variable,  m(t),  defined  as 


m(0  =  ^^(0  =  w«(t) 


(3.22) 


is  modelled  as  being  uncorrelated  in  time.  However,  if  an  object  is  executing  a 
manoeuvre  at  time  t,  it  is  likely  to  be  executing  a  similar  or  perhaps  even  the  same 
manoeuvre  at  time  t  +  r  for  sufficiently  small  t  which  implies  that  m{t)  should  be 
modelled  as  a  time-correlated  process.  One  approach,  [27,  71,  75]  is  to  model  each 
component  of  m(t)  =  (mi(t),m2(t),m3(<)]^  as  a  first-order  Gauss-Markov  process 
with  autocorrelation  function  r,(T)  given  by 


r,(T)  =  ^{m,(t)m.(<  +  t)}  =s  <7je~“‘*l’’l. 


(3.23) 


where  is  the  variance  of  the  ith  component  of  the  manoeuvre  variable,  and  a^.  >  0 
is  the  reciprocal  of  the  manoeuvre  correlation  time  constant. 

The  subscripts  i  on  and  will  be  omitted  since,  in  the  absence  of  alterna¬ 
tive  information,  each  component  of  ni(t)  can  be  treated  identically.  The  Wiener- 
Kalmogorov  whitening  procedure  can  then  be  used  to  express  the  manoeuvre  variables 
mi(i)  in  terms  of  white  noise  by  taking  the  Laplace  transform  of  r,(T)  and  writing 

ft(j)  ^  £(ri(r)) 

— 2Q|<7f 


The  function 


(s  -  Q,)(s  -h  at) 

1 


s  +  0| 


(3.24) 

(3.25) 


is  identified  as  the  Laplace  transform  of  whitening  filters  for  each  m,(t),  while 

VV;(s)  =  2a, (3.26) 
UNCLASSIFIED  DRES-SR-577 


37 


represents  the  power  spectral  density  of  independent  white  noise  processes  Wi{t)  that 
drive  identical  filters  H{3)  to  produce  rni{t),  according  to  first-order  differential  equa¬ 
tions 


mi{t)  =  -atmi(t)  -f- 


(3.27) 


With  this  model  for  manoeuvring  motion,  the  state  vector  Xt  in  (3.19)  is  aug¬ 
mented  to  include  m(<)  of  (3.22), 


X, 


and  propagates  in  time  according  to  (3.20),  but  now 


OsNpXS 

IsNp 

,  and  G|  = 

OsJVpxS 

03x3Np 

-Oils 

I3 

(3.28) 


(3.29) 


and  w,(t)  is  zero-mean,  temporally  white  Gaussian  noise  wit’i  covariance  Qj  = 
2at<Tfh- 

The  linear  differential  equation  in  (3.20),  with  the  definitions  in  (3.29),  can  be 
written  as  a  discrete-time  difference  equation  using  standard  techniques  to  give 

x,(it  +  1)  =  $,(fc)x«(fc)  +  wf(k),  (3.30) 


where 

$,(ifc)  =  e^‘<‘*+>-‘*>.  (3.31) 

The  state  transition  matrix,  $«,  can  be  written  in  closed  form  due  to  the  simple 
form  of  A<,  [71].  The  system  is  driven  by  a  zero-mean,  discrete-time,  white  noise 
sequence,  wf(k),  with  covariance  Qf  which  may  be  computed  (see  [71])  directly  from 
Equation  (B.35)  of  Appendix  B,  or  approximated,  assuming  the  sample  period  is 
much  smaller  than  system  time  constants  (2aT  1,  [71])  as 

Qf  «  2ata^TGtGj,  (3.32) 

where  T  is  the  sample  period  (superscript  ‘T’  still  denotes  transposition). 

Three  parameters,  Np,  ai,  and  ^i,  must  be  specified  to  complete  the  trsmslational 
motion  model.  Extensive  simulations  have  demonstrated  that  Np  =  2  provides  a  rea¬ 
sonable  balance  between  accuracy  and  computational  requirements.  In  fact,  because 
measurements  are  related  directly  only  to  object  position,  estimation  of  higher  time 
derivatives  {Np  >  2)  often  demonstrated  serious  lag  and  overshoot  which  can  lead 
to  filter  instability.  The  parameter  ai  is  the  reciprocal  of  the  manoeuvre  correlation 
time  constant.  If  manoeuvres  are  expected  to  occur  only  over  short  time  periods,  at 
should  be  chosen  fairly  large  (>  1),  whereas  slower  expected  manoeuvres  are  modelled 
with  a  much  smaller  at  (%  1/60).  In  the  selection  of  erf,  Singer  [71]  proposed  a  model 
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for  the  probability  distribution  of  each  m,(<):  the  maximum  and  minimum  values, 
A^max  and  — Mm»v,  respectively,  each  occur  with  probability  Pn»ax;  mj  =  0  occurs  with 
probability  Pq.,  and  m,  takes  on  values  in  the  range  (Mma,,  -A/m.,)  according  to  the 
appropriate  uniform  distribution.  In  this  case, 

A/2 

=  (3.33) 

Although  this  discussion  may  provide  some  guidance  in  the  selection  of  parameters, 
at  and  in  particulcir  can  be  treated  as  tuning  parameters  in  the  filtering  equations. 
The  first-order  Gauss  Markov  model  is  also  used  in  the  parameterizations  of  rotational 
motion  developed  in  the  following  sections. 


3.5  Rotational  Motion 

The  object’s  instantaneous  orientation  with  respect  to  the  earth-fixed  frame  is 
given  by  the  basis  transformation  where  elements  of  the  vector  ^  depend  on 

the  parameterization  selected  for  rotational  motion.  The  orthogonal  trainsformation 

belongs  to  the  three-dimensional  rotation  group  for  which  there  are  about  eight 
commonly  used  parameterizations  [67].  Appendix  A  provides  a  review  of  the  Eu¬ 
ler  angle-axis,  quaternion,  and  roll-pitch-yaw  parameterizations  and  equations  which 
govern  their  temporal  behavior. 

A  state  vector,  Xr  €  corresponding  to  rotational  motion  is  defined  which 
contains  elements  of  ^  and  additional  states  which  describe  the  temporal  behavior  of 
The  dynamical  model  for  rotational  motion  is  written  as 

Xr(0  =  fr[Xr(0]  +  G,Wr(<),  (3.34) 

where  /,[•]  is  a  vector- valued  function,  and  the  zero-mean  Gaussian  white  noise  process 
Wr(<)  is  mapped  to  the  rotational  motion  state  space  through  the  matrix  G,.  In 
the  angle-axis  and  roll-pitch-yaw  parameterizations,  approximate  temporal  behavior 
results  in  linear  dynamic  models,  however,  the  quaternion  formulation  results  in  a 
nonlinear  model. 

In  navigation  systems,  measurements  of  angular  velocity  are  often  provided  by 
rate  gyroscopes.  Applications  in  navigation,  therefore,  commonly  parameterize  rota¬ 
tional  motion  directly  in  terms  of  angular  velocity.  The  angular  velocity  of  Fq  with 
respect  to  Fe  expressed  in  Fe,  denoted  is  related  to  and  its  time  derivative 

according  to 

(ifi)  =  (3.35) 

where  [wj*  denotes  the  matrix  cross-product  operator  corresponding  to  the  vector 
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w  =  is  defined  as 

0  —11)3  ^ 
t03  0  — IPi 

— tuj  ti)i  0 


(3.36) 


When  no  confusion  can  result,  the  angular  velocity  will  be  simply  written  as  w  = 

Target  tracking  problems,  however,  are  rarely  provided  with  estimates  of  angu¬ 
lar  velocity,  but  instead  must  estimate  w(f)  based  on  angular  position  information 
embedded  in  the  available  measurements.  Only  the  quaternion  parameterization  is 
formulated  directly  in  terms  of  the  angular  velocity;  dynamical  models  for  angle-axis 
and  roll-pitch-yaw  parameterizations  employ  time  derivatives  of  rotation^  parameters 
which,  in  turn,  can  be  used  to  compute  a  suboptimal  estimate  of  angular  velocity. 
Even  in  the  quaternion  formulation,  an  extended  Kalman  filter  must  be  employed 
and  hence  suboptimal  performance  can  be  expected.  Sections  3.5.1,  3.5.2,  and  3.5.3 
present  and  discuss  dynamical  models  for  the  angle-axis,  quaternion,  and  roll-pitch- 
yaw  filters,  respectively. 


3.5.1  Angle- Axis  Parameterization 

An  orthogonal  basis  transformation  inlR^can  be  defined  by  an  orientation  vector 
(e  =  whose  direction, 

^  iilf.  (3.37) 

specifies  the  axis  of  the  rotation  operation  relating  the  two  reference  frames,  and 

magnitude, 

^  =  llfell.  (3.38) 

specifies  the  angle  through  which  the  coordinate  system  is  rotated  about  (e,  with 
the  direction  of  rotation  taken  by  the  right-hand  rule.  Since  elements  of  ^e  ^ 
invariant  under  this  rotation  operation,  the  subscript  will  be  omitted.  The  basis 
transformation  in  this  case  is  computed  as 

Ig  =  exp(f') 

=  l3  +  8in(7)^-f(l-co8(7)]  ((■)*,  (3.39) 

where  is  the  matrix  cross  product  operator,  defined  in  (3.36),  associated  with 

The  axis,  of  is  generally  distinct  from  what  is  commonly  called  the  **axis  of 
rotation”  or  instantaneous  angular  velocity', 

w  =  7^  -I-  sin  7^  -h  (1  -  cos  7)^1,  (3.40) 

'All  time  derivatives  are  relative  to  an  observer  in  the  earth-fixed  frame,  and  derivative  vectors 
are  expressed  in  the  earth-fixed  frame. 
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which  has  components  on  three  orthogonal  axis  and  If  the  axis  (  is  fixed, 
(3.40)  reduces  to  the  simple  expression  w  =  However,  even  if  the  object  has 

constant  angular  velocity,  the  axis  of  Vg  need  not  remaun  fixed. 

Under  an  assumption  of  “smooth”  object  motion,  approximate  temporal  behavior 
of  the  orientation  vector  (  over  any  sample  period.,  {<|<*  <  <  <  tfc+i},  is  expressed  in 
terms  of  a  first-order  Gauss-Markov  process  in  a  manner  equivalent  to  that  employed 
for  translational  motion  in  Section  3.4.  In  this  case  the  rotational  motion  state  vector, 
X,  €  IR^’’,  where  Nr  =  3(N(  +  1),  is  defined  as 


X, 


A 


and  propagates  in  time  according  to 


Xr{t)  =  A,x,(t)  4-  G,w,{t), 


(3.41) 


(3.42) 


where 


and  Wr(f)  is  zero-mean,  temporally  white  Gaussian  noise  with  covariance  Qr  = 
20x0^13,  and  the  roles  of  the  reciprocal  manoeuvre  correlation  time  constant,  Or, 
and  the  variance,  <7^,  of  the  manoeuvre  variable  are  equivalent  to  that  described  in 
the  development  of  the  translational  motion  model. 

A  primary  difficulty  with  this  approach  is  that  the  orientation  vector  (  must  be 
reset,  first  to  maintdn  a  finite  magnitude  (rotation  angle)  when  the  object  continually 
rotates  in  one  direction,  and  second  to  yield  decreasing  Cramer  Rao  bounds.  A 
trajectory  ^{t)  in  the  region  x  <  ||(||  <  2x  is  equivalent,  in  the  sense  that  it  produces 
the  same  rotation  matrix,  to  a  trajectory  C(0  in  the  re^on  ||C|i  <  x  if 

C(0  =  (11^(011  -  2x)^(<)  =  ^(0  -  2x^(0.  (3.44) 


A,= 


Osxsjv^ 


-ttfis 


and  Gr  = 


Os/Vjxs 

Is 


This  transformation  represents  a  reflection  through  the  origin  of  IR^,  followed  by  a 
reflection  in  the  plane  tangent  to  the  sphere  of  radius  x  at  the  point  — x^.  One 
difficulty  is  that  if  I  is  non-constant,  then  C(0  I^nve  an  infinite  number  of  non¬ 
zero  time  derivatives.  Let  the  orientation  vector  immediately  before  the  reset  be 
denoted  by  and  the  corresi>onding  orientation  vector  immediately  following  the 
reset  :  denoted  by  C*  An  approximate  impulsive  reset  which  limits  7  to  the  range 
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[0,  it]  can  be  applied  according  to  the  algorithm  (given  only  to  the  first  2  derivatives) 
If  m  >  tt)  then 

7  =  IKII  {Preliminary  Calculations} 

e  =  Ui 
i  =  ei 
i  =  (h-(f}(h 

(  =  (u  -  muh  -  (W)  -  iir + u  )(h 

C  =  i  ~  {Reset  Procedure} 

C  =  ^-2t| 

C  = 


where,  ^^s  indicated,  the  approximate  reset  can  be  extended  to  further  time  derivatives 
by  differentiating 

C  =  (7  -  2»){  =  {  -  (3.46) 

This  model  is  completely  specified  by  three  parameters,  N^y  a,,  and  a^y  which  can 
be  selected  based  on  the  discussion  of  translational  motion  parameters  in  Section  3.4. 
Further  discussions  of  the  reset  and  expected  performance  of  the  angle-axis  filter  are 
deferred  to  Chapter  4  which  compares  Cramer  Rao  bounds  and  presents  simulation 
results. 


3.5.2  Quaternion  Parameterization 


A  quaternion  [29]- [37],  [65]- [70],  [83],  [84],  is  a  four  dimensional  hypercomplex 
number’ 


L94  J 


9i*  +  927  +  93^  +  94» 


(3.47; 


which  is  expressed  in  terms  of  basis  elements  consisting  of  the  real  number  -f  1,  and 
three  imaginary  units  t,7,  ib,  which  satisfy 


,•2  =  j2  =  It’  =  -1, 

ij  =  -ji  =  ky 

jk  =  —kj  =  iy  and 
ki  =  —ik  =  j. 


(3.48) 


’Some  authors  place  the  scalar  quaternion  element,  q^,  in  the  first  position  while  others  place  the 
scalar  element  in  the  fourth  position,  as  is  the  case  here,  when  representing  the  quaternion  in  vector 
form. 
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If  lE(q)  rotates  ffi?  through  an  angle  7  about  the  axis  (  =  to  bring  Fe 

into  alignment  with  Fo,  the  unit  quaternion  defined  by 


=  [-(1  sin(|),  -6  sin(|),  -^3  8in(^),coe{|)j 

gives  Ig(q)  as 

ii(i)  =  jj^r(q)  =  r(q), 

where 


9?  -92-93  +  94  2(9192  +  9394)  2(9193  -  9294) 

2(9193  -  9394)  -9?  +  92  -  93  +  94  2(9293  +  9194) 

.  2(9193  +  9294)  2(9293-9194)  -91  -92  +  93  +  94 


(3.49) 

(3.50) 


(3.51) 


The  matrix  r(q)  represents  combined  operations  of  an  orthogonal  transformation  (I^) 
and  uniform  scaling  by  ||q|p.  The  quaternion  parameterization  is  four-dimensional 
and  IS  often  fixed  with  a  quadratic  constraint  of  the  form  ||q||  =  1  since  depends 
only  on  the  normalized  quaternion  q.  Such  constr^unts  are  not  easily  incorporated 
into  the  Kalman  filtering  equations  due  to  the  linear  structure  of  the  filter. 

A  common  approach,  [29]-[37),  [53],  is  to  implement  the  filter  based  on  unit  quater¬ 
nions  and  employ  an  impulsive  normalization  of  the  estimated  quaternion,  using  stan¬ 
dard  Kalman  filtering  notation  from  Appendix  B, 

\m\k)r 

immediately  following  each  observation  event.  In  the  present  work,  this  approach 
resulted  in  strict  and  total  divergence  of  angular  velocity  estimates  in  a  significant 
portion  (sometimes  more  than  80%)  of  simulation  runs.  Tahk  and  Spayer  [66]  propose 
to  incorporate  soft  or  stochastic  constraints  directly  into  the  measurement  model  and 
apply  the  extended  Kalman  filter  to  the  augmented  system.  This  approach,  although 
it  resulted  in  an  improvement  over  impulsive  normalization,  also  led  to  poor  and 
often  divergent  behavior  in  angular  velocity  estimation.  Bar-Itzack  and  Oshman  [65] 
treat  the  problem  of  estimating,  in  the  current  notation,  a  unit  quaternion  q,  and 
hence  l£(q),  from  sequences  of  observations  of  corresponding  vectors  {(r£,r^);t  = 
1 , 2, . . . ,  Af },  and  angular  velocity  w  over  time  with  an  extended  Kalman  filter.  With  a 
first-order  approximation,  their  approach  is  to  propagate  the  estimated  quaternion  in 
the  filter  without  impulsive  normalization,  which  means  that  the  estimated  quaternion 
need  not  have  unit  norm.  For  output  purposes  only,  the  estimated  quaternion  is 
normalized  through  division  by  its  Euclidean  norm  [64].  Their  problem  is  slightly 
different  from  the  one  presently  under  consideration,  but  the  miun  ideas  of  their 
approach  have  been  employed  in  the  quaternion  filter  with  slight  modifications. 


(3.52) 
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The  dynamical  model  is  based  on  the  assumption  that  the  quaternion  has  constant 
norm  (not  necessarily  unity)  so  that 

q(0  =  llq(01lq(0-  (3.53) 

The  unit  quaternion,  q(t)  defined  in  (3.49),  propagates  in  time  (see  Appendix  A) 
according  to  the  differential  equation 

4(0  =  n(w(0M0,  (3-54) 

where  w  =  [w^Je  =  and 


0 

—W3 

U)2 

t£>3 

0 

-Wi 

—W2 

— U)2 

Wi 

0 

—W3 

Wi 

U>2 

U>3 

0 

(3.55) 


Multiplying  both  sides  of  (3.54)  by  ||q||  gives,  with  (3,53), 

q(t)  =  n[w(<)]q(t).  (3.56) 


Once  again  employing  the  assumption  that  rotational  motion  is  ‘‘smooth”,  tem¬ 
poral  behavior  of  w  over  any  sample  period  is  approximated  by  a  first-order  Gauss- 
Markov  process  in  a  manner  equivalent  to  that  employed  for  translational  motion  in 
Section  3.4.  A  state  vector,  Xu,,  for  the  angular  velocity  and  its  time  derivatives  is 


defined  as 


(3.57) 


which  propagates  in  time  according  to 


x«,(0  =  A„,x„(/)  -1-  G„w,.(<), 


(3.58) 


where 

A.  =  f  H  1 ,  and  G.  =  [  1 ,  (3.59) 

.  03x3^.  -Oris  j  I  J 

and  Wr(t)  is  zero-mean,  temporally  white  Gaussian  noise  with  covariance  Qr  = 
2ar<Trl3,  and  the  roles  of  the  reciprocal  manoeuvre  correlation  time  constant,  0^ 
and  the  variance,  of  the  manoeuvre  variable  are  equivalent  to  that  described  in 
the  development  of  the  translational  motion  model. 

An  overall  state  vector,  Xr,  corresponding  to  rotational  motion  is  defined  as 


(3.60) 
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which  propagates  io  time  according  to  the  nonlinear  differential  equation 


Xrit)  =  i 

T[Xr(0]  +  GrWrI 

(0 

= 

■n[w(f)l  0 

0  Au, 

Wr(f). 

(3.61) 

Time  propagation  in  the  extended  Kalman  filter  requires  numerical  integration  of 
(3.61)  with  GrW,(t)  =  0. 

If  w  is  approximately  constant  over  any  sample  period,  an  approximate  discrete¬ 
time  difference  equation  can  be  obtained  for  x,  of  the  form 


Xrik  +  1)  =  9rik)Xr{k), 

(3  62) 

where 

♦,(ifc)idiag[#,(l:),Mi^)], 

(3.63) 

and 

♦,(k)  i  exp(n[w(fc)]{f*+i  -  tk}) 

(3.64) 

*u,(k)  =  exp(A,„{<fc+i  - 1*}). 

(3.65) 

A  closed-form  expression  for  4^,  (see  Appendix  A)  is 

*,{k)  =  CO.  I,  +  .in  ijljjniwl.  (3.66) 

Because  n[w]  is  a  skew  symmetric  matrix,  ^q{k)  is  orthogonal^  and  hence  ||q(Jk  -|- 
1)11  =  “  required. 

Numerical  integration  in  (3.62)  is  based  on  the  assumption  that  w  is  approxi¬ 
mately  constant  over  the  sample  period.  This  will  be  essentially  true  provided  either 
w  is  constant,  or  the  sample  period  is  very  small.  An  assumption  of  small  sample 
periods  is  often  employed  in  motion  analysis,  but  is  very  restrictive.  Moreover,  this 
approximation  does  not  exploit  estimates  of  time  derivatives  of  w.  Many  standard 
methods  for  numerical  integration  of  the  quaternion  differential  equation  are  designed 
for  navigation  systems  in  which  measurements  of  angular  velocity  are  available  di¬ 
rectly  [84].  The  following  algorithm  is  similar  in  form  to  one  analyzed  by  Branets 
and  Shmyglevski  [83].  This  approach  employs  the  modelled  temporal  behavior  of  w 
and  the  most  recent  estimates  of  w  and  its  time  derivatives. 

Define  a  partition 


n(*)  =  {f*  =  ro  <  Ti  <  •  •  •  Tat,  =  4+1 }  (3.67) 

’The  matrix  exponential  of  a  akew  aymmetric  matrix  u  always  orthogonal. 
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of  the  Jtth  sample  period,  and  assume  for  convenience  that  —  t/  =  t  is  constant. 
With  the  linear  model  for  x^,, 

w(t/)  =  C„,x„(t/)  (3.68) 

where 

and 

i,o,o,o,...,o  ■ 

C«,  =  0, 1,0,0,. ..,0  .  (3.69) 

0,0,1, 0,...,0 

Provided  r  is  chosen  small  enough,  or  that  is  chosen  large  enough,  so  that  w  is 
approximately  constant  over  any  interval  of  the  partition  n(ib),  time  propagation  of 
q  over  the  complete  sample  period  may  be  approximated  by 

q(l  +  1)  =  #,{t)q(l),  (3.70) 

where  ^ 

♦,(‘)  =  l  ii  “P  {n  (c«  (♦-(>■))'  *.(<*)]  ’■} .  (3.71) 

in  which  the  notation  i  f]  indicates  that  the  index  i  decreases  from  left  to  right  in 
the  matrix  product,  0(-] :  IR^  -♦  according  to  (3.55),  and  (3.66)  can  be  used  to 
evaluate  the  matrix  exponentials.  Note  that  in  (3.71)  is  a  product  of  orthogonal 
matrices  and  hence  is  orthogonal,  which  means  that  ||q(ib  +  1)||  =  ||q(ii^)||- 

It  should  be  noted  that  the  continuous-time  form,  ^r(t,  A),  of  9r{k)  obtained 
by  replacing  tk+i  with  t  and  ik  by  A  in  (3.64)  or  (3.71)  and  (3.65),  is  not  a  '*state 
transition  matrix”  in  the  usual  sense  because  it  does  not  satisfy  the  fundamental 
property 

=  fel».(l,n),  (Wse).  (3.72) 

As  a  result,  even  though  may  be  used  for  approximate  time  propagation  of  the 
state  estimate,  this  matrix  cannot  be  used  to  propagate  the  Kalman  covariance  ma¬ 
trix  over  time.  At  any  measurement  event,  the  measurements  are  directly  related 
to  the  quaternions  and  not  to  the  angular  velocity.  The  Kalman  filter  must  use  the 
dynamical  model  to  extract  information  about  angular  velocity  from  prediction  er¬ 
rors  which,  in  turn,  depend  on  the  quaternion  estimates.  Time  propagation  of  the 
Kalman  covariance,  therefore,  must  muntain  the  interaction  between  quaternions  and 
the  angular  velocity  specified  in  (3.56).  The  block  diagonal  matrix  in  (3.63)  does 
not  explicitly  model  the  interdependence  of  q  and  w.  Approximate  time  propaga¬ 
tion,  [29]-[37],  [53],  of  the  covariance  matrix  is  performed  with  an  approximate  state 
transition  matrix  which  employs  the  rotational  motion  component  as 

♦.(*)  =  exp(r,(*r)|lMi  -  M),  (3.73) 
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where  Fr(fc)  is  the  Jacobian  of  fr[Xr(fc)], 


and 


Fr(*)  ^ 


^r[Xr] 

dXr 

nlw(i)l  [qM  0] 


—94  93  —92 
—93  ~94  9i 
92  —91  -94 
9i  92  93 


(3.74) 


(3.75) 


Note  that  $r(^)  in  (3.73)  need  not  in  general  maintain  constant  quaternion  norm  and 
hence  should  not  be  used  in  time  propagation  of  the  state  estimate. 

The  quaternion  filter  has  been  implemented,  using  standard  Kalman  filtering  no¬ 
tation  introduced  in  Appendix  B,  with  the  following  approach: 


1. 

2. 

3. 

4. 


The  filter  is  initialized  with  a  unit  quaternion,  ||q(0|  —  1)||  =  1; 

The  norm  of  the  estimated  quaternion  is  invariant  under  time  propagation,  i.e., 
I|q(*  +  l|*)ll  =  llq(m  it  =  1,2,3,...; 

The  measurement  model  employs  r(q)  of  (3.51)  as  the  transformation  from  Fq 
to  Fe  instead  of  the  strictly  orthogonal  matrix 


For  output  purposes  only,  the  estimated  unit  quaternion  is  taken  as 

^(jb|it)  t 


llq(<^l*)ll’ 


(3.76) 


and,  again  for  output  purposes  only,  structure  estimates  denoted  *r|7(ib|ib)  are 
taken  as 

■fo(*W  =  llq(*l*)rfo(‘l*).  (3.77) 

for  <  =  1, 2, . . . ,  Nfj  where  the  vectors  ^(ib|k)  are  structure  estimates  generated 
within  the  filter; 

5.  The  estimation  error  covariance  matrix,  P*(ib|ib),  for  the  filter  output  is  taken 
as 

P-(/b|*)  i  (3.78) 

where  P(k|ib)  is  the  estimated  Kalman  covariance,  and 

?  S  diH  |l«,  Ilq(*l»)ll’l»«,} .  (3.79) 
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This  approach  parallels  that  proposed  by  Bar-Itzhack  and  Oshman  [65].  Due  to 
Item  3  above,  the  measurement  model  depends  on  the  product  of  r(f )  and  structure 
vectors  internal  to  the  filter  which  ap]>eaTs  in  the  expression, 

4  =  r(,)rb  +  [Tgls.  (3.80) 

The  innovation  sequence  in  the  quaternion  filter,  therefore,  depends  on  the  product 

IfSls  =  (3.81) 

If  the  quaternion  filter  works  well,  [rj)]£  — » IrJ>]s  for  t  =  1,2, . ,  .,Nj,  even  though  q 
may  not  have  unit  norm,  and  may  not  converge  to  a  true  structure  vector  (due  to 
the  scaling  effect  of  r(q)).  Noting  from  (3.50)  and  (3.51)  that 

[rb]£  =  r(q)fb 

=  {lg(q)H’fb(t|*)}  (3-82) 

leads  naturally  to  the  extraction,  in  item  4  above,  of  a  unit  quaternion  and  appropriate 
structure  estimates. 

This  quaternion  model  is  completely  specified  by  four  parameters,  o,,  cr*, 
and  Npi.  The  first  three  of  these  can  be  selected  based  on  the  discussion  of  transla¬ 
tional  motion  parameters  in  Section  3.4.  Selection  of  number  of  intervals,  Ni,,  in  the 
partition  of  each  sample  period  should  also  be  based  in  part  on  the  expected  com¬ 
plexity  of  rotational  motion.  Investigations  leading  to  the  research  reported  herein 
indicated  that  =  b  with  a  sample  period  of  T  =  0.1  seconds  gives  reasonable  accu¬ 
racy  in  ideal  time  propagation  of  the  state  at  the  expense  of  increased  computational 
complexity, 

3.5.3  Roll  Pitch  Yaw  Parameterization 

The  roll-pitch-yaw  (RPY)  representation  [38, 63, 82]  is  a  particular  case  of  an  Euler 
angle  parameterization  of  the  three-dimensional  rotation  group.  The  roll,  pitch,  and 
yaw  angles,  denoted  9,  and  V**  respectively,  define  an  ordered  sequence  of  three 
plane  rotations  which  can  be  used  to  express  as 

=  exp(0ej)  exp(^eS)  exp(  ),  (3.83) 

where  e^,  i  =  1,2,3,  are  the  standard  basis  vectors  oflR^.  Expanding  the  exp>onentials 
with  (A.  13)  gives  three  plane  rotations, 

10  O' 

0  cos(^)  —  sin(4i)  , 

0  sin(^)  cos(^) 


exp(^J)  = 
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exp(de2) 

exp(V>ei) 


cos(^>) 

0 

—  sin(d) 

C08(^) 

sin(V’) 

0 


0  sin(0) 

1  0 
0  cos(tf) 

-  8in(0)  0 

CO8(0)  0 

0  1  . 


and 


(3.84) 


The  angles  <f>,  0,  and  V’  have  positive  sense  defined  by  the  right  hand  rule  about  their 
respective  rotation  axis  (see  Appendix  A). 

It  has  been  reported,  [28,  32,  37],  that  major  difficulties  arise  with  this  approach 
because  the  sequence  of  plane  rotations  are  not  defined  about  orthogonal  axis.  This 
property  results  in  highly  nonlinear  dynamical  models  if  rotational  motion  is  formu¬ 
lated  directly  in  terms  of  angular  velocity.  The  angular  velocity  w  =  [w^]^,  is  given 
in  terms  of  time  derivatives  of  roll,  pitch,  and  yaw  angles  as 


w  =  3rpy 


(3.85) 


where 


3flPY  ~ 


co6(V’)  co8(f>)  —  sin(0)  0  ' 
sin(V*)  coe(d)  cos(^)  0 

—  8in(d)  0  1 


(3.86) 


The  mathematical  singularities,  0  =  ±x/2,  characterize  orientations  where  the  deter¬ 
minant  of  the  Jacobian  matrix  detf^Apy]  =  —  co6(^)  vanishes. 


The  parameter  vector  for  the  roll-pitch-yaw  filter  is  defined  as 


(3.87) 


Under  an  assumption  of  ‘‘smooth”  object  motion,  an  approximate  temporal  behavior 
of  C  over  any  sample  period,  {<|<k  <  <  <  <fc+i),  is  expressed  in  terms  of  a  first-order 
Gauss-Markov  process  in  a  manner  equivalent  to  that  employed  for  translational 
motion  in  Section  3.4.  In  this  case  the  rotational  motion  state  vector,  x,  €  IR^', 
where  Nr  =  3{N(  -f- 1),  is  defined  as 


and  propagates  in  time  according  to 

Xr(i)  =  ArXr(i)  -h  G,Wr(<). 


(3.88) 


(3.89) 


where 


A, 


03N<x3 

OsxSWj  —Oris 


and  Gr  = 


OsWfxs 

h  ’ 


(3.90) 
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zuid  Wr(()  is  zero-mean,  temporally  white  Gaussian  noise  with  covariance  Qr  = 
2ar(7^l3,  and  the  roles  of  the  reciprocal  manoeuvre  correlation  time  constant,  a^, 
and  the  variance,  of  the  manoeuvre  variable  are  equivalent  to  that  described  in 
the  development  of  the  translational  motion  model. 

The  roll- pitch-yaw  model  is  completely  specified  by  three  parameters,  N(^  Qr, 
2Lnd  which,  again,  can  be  selected  based  on  the  discussion  of  translational  motion 
parameters  in  Section  3.4. 

3.6  Initial  Estimates 

The  extended  Kalman  filter  requires  an  initial  estimate  of  the  state,  x(0|  —  1), 
and  characterization  of  the  initial  uncertainty,  P(0|  —  1),  in  this  estimate.  Broida  and 
Chellappa,  for  example,  have  studied  a  batch  approach  [30,  31 ,  32,  37]  which  employs 
the  Conjugate  Gradient  descent  technique  from  the  IMSL  library.  Simulation  results 
shown  by  Broida  et  ai,  [32],  employ  what  are  called  "crude”  initial  estimates  obtained 
from  the  first  10  image  frames  with  75  iterations  of  the  batch  technique. 

In  future  work  and  eventual  applications,  it  is  expected  that  initialization  of  object 
tracking  filters  would  be  based  on  information  from  lower  subsystems  in  a  hierarchical 
structure  such  as  that  described  in  Section  1.2.  For  the  purpose  of  this  investigation, 
a  simple  single-frame  initialization  provides  suitable  estimates  of  position,  orienta¬ 
tion,  and  structure,  while  all  higher-order  motion  parameters  (time  derivatives)  are 
initialized  as  zero.  The  initialization  algorithm  is  described  as  follows: 

Step  1  Obtain  an  estimate  of  the  3D  positions  of  three  or  more  feature  points  with 
respect  to  the  earth-fixed  frame  through  steriopsis  based  on  noisy  measurements 
in  multiple-camera  images  and  assumed  correspondence  information; 

Step  2  Select  three  noncoUinear  feature  points  from  all  available  3D  points; 

Step  3  Elstimate  the  initial  position  of  the  object,  form  an  orthonormal,  right-handed 
triad,  Fq,  in  Fe  from  the  three  special  3D  points  based  on  constrsunts  detailed 
in  Section  3.2  and  illustrated  in  Figure  3.2,  and  fix  the  origin,  Oo,  of  Fq  with 
respect  to  the  3D  feature  points  by  specifying  the  vector  d; 

Step  4  Extract  a  valid  set  of  orientation  parameters  from  the  basis  vectors  of  Fq 
expressed  in  Fg; 

Step  5  Express  positions  of  all  feature  points  and  d  with  respect  to  Fo- 

This  initialization  is  concerned  only  with  obtaining  an  initial  estimate  of  the  state; 
the  initial  covariance  matrix  is  selected  as  diagonal  assuming  significant  initialization 
errors  (furly  large  diagonal  entries). 
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Step  1:  Given  Nf  >  Z  feature  points  visible  in  each  of  Nc  >  2  cameras  with 
spatial  correspondence  information,  the  objective  is  to  estimate  r^,  i  =  1,2,...,  N/. 
The  position  of  the  ith  feature  point  in  the  image  plane  of  the  jth  camera  is  written, 
from  (2.19),  as 


(3.91) 


With  (3.91)  and 


so  that  =  0  and 


A  Uj  -I,  0 

V}  0  -Li 


(3.92) 


(3.93) 


where  and  are  known  for  j  =  l,2,...,iVc,  a  system  of  linear  equations 

can  be  written  in  the  form 


D'igTi  =  -D5[Ti'lo„  i  =  l,2,...,JVc. 


(3.94) 


Forming  two  matrices 


M’  = 


Dii?. 

Djif. 


,  and  N’  = 


Di|T|k 

DilTllft 


(3.95) 


immediately  gives  the  least-mean-squared  estimate  of  as 


(3.96) 


for  the  tth  feature  point.  This  process  is  repeated  for  each  of  the  Nj  feature  points. 

Step  2:  Many  criteria  could  be  specified  for  selecting  the  best  set  of  three  special 
noncollinear  feature  points  from  all  available  f^.  Moreover,  this  selection  might  be 
juded  by  decisions  from  lower-level  subsystems  of  a  hierarchical  approach  such  as  that 
discussed  in  Section  1.2.  A  simple  approach  is  to  select  those  three  points  for  which 
the  area  of  the  triangle  (see  Figure  3.2)  formed  by  joining  the  three  3D  points  is 
maximized,  thus  maximizing  a  measure  of  the  degree  of  noncollinearity  and  distance 
between  points.  This  can  be  done  by  selecting  3  indices,  {ti,ii,*3}  C  {1,2, ...,iV/} 
such  that, 

II  [«  -  f»]'  [f‘i  -  ri]  f  (3.97) 
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is  maximized.  Since  the  order  of  selection  is  unimportant,  there  are 


(  Nf\  ^  Nf\ 

V  3  )  V.{Nf-3)\ 


(3.98) 


combinations  for  comparison. 

Step  3:  The  estimated  initial  position,  p(0)  =  [T§]£:(0),  of  the  object  is  taken  as 
the  sample  mean  of  all  estimated  feature  point  positions: 


1 

»=1 


(3.99) 


The  three  feature  points  selected  in  Step  2  are  ordered,  arbitrarily  in  the  absence  of 
prior  information,  as  {r^,  r'f ,  }  according  to  the  constraints  detailed  in  Section  3.2. 

The  object-centred  frame  is  fixed  on  the  object,  and  the  initial  estimates  of  orientation 
are  provided  with 


[dolfi  =  rg-p(O), 

yj  ^  ft  - 
[IcoIe  =  Me  Me* 


(3.100) 

(3.101) 

(3.102) 

(3.103) 


Step  4:  Initial  estimates  of  orientation  parameters  (angle-axis,  quaternion,  or 
roll-pitch-yaw)  are  extracted  from  the  initial  estimate  of  the  basis  transformation 


li  =  [(iolfij  liojc,  (kol^l » 


(3.104) 


according  to  methods  outlined  in  Appendix  A. 

Step  5;  Initial  structure  estimates  with  respect  to  Fo  are  computed  with 


do  =  Ig  [do]£,  and 

To  =  iT(fk-MO)),  *  =  1,2,...,/V/. 


(3.105) 

(3.106) 


By  construction,  fg  =  do,  ro,2  =  and  =  do^,  where  do 

is  now  treated  as  a  known  constant  vector;  these  six  structural  parconeters  are  not 
estimated  by  the  filters. 
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3.7  Chapter  Summary 

This  chapter  has  introduced  structure,  translational  motion,  rotational  motion, 
and  measurement  state  space  models  for  manoeuvring  objects  observed  with  a  multiple- 
camera  imaging  system.  Both  translational  and  rotational  motion  are  described  with 
first-order  Gauss-Markov  processes  which  are  completely  defined  by  selection  of  a 
small  number  of  parameters:  the  number  of  time  derivatives  {Nt  and  one  of  N(,  Ny,, 
or  N();  the  reciprocal  manoeuvre  correlation  time  constants  (a^  and  Or);  and  the 
manoeuvre  variances  (<r?  and  <r^).  The  quaternion  filter  also  requires  selection  of 
the  number  of  intervals,  TV,,  of  each  sample  period  for  time  propagation  of  the  state 
estimate,  and  all  filters  require  estimation  of  the  measurement  noise  covariance,  R. 
A  simple  single-frame  initialization  algorithm  has  been  proposed  for  object  position, 
orientation,  and  structure.  Chapter  4  discusses  parameter  selection,  measures  of  op¬ 
timal  performance  provided  by  Cramer  Rao  bounds,  and  presents  simulation  results 
and  performance  comparisons  for  filters  formed  from  the  three  parameterizations  of 
rotational  motion. 
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4.  Performance  With  Simulated 
Imagery 


The  previous  chapter  introduced  motion,  structure  and  measurement  models  based 
on  three  parameterizations  of  rotationaJ  motion.  In  this  chapter,  relative  performance 
of  three  iterated  extended  Kalman  filters,  each  based  on  a  single  parameterization, 
is  examined  and  demonstrated  through  presentation  of  a  subset  of  results  generated 
from  extensive  Monte  Carlo  simulations.  One  limitation  of  analysis  based  on  simu¬ 
lated  imagery  is  that  the  assumed  central  projection  image  formation  model  is  exact; 
that  is,  the  measurement  model  is  based  on  the  central  projection  transformation,  and 
simulated  imagery  is  generated  precisely  from  this  model.  This  approach,  which  is 
common  to  most  simulation  studies  in  image  analysis,  is  only  valid  if  distortion  effects 
in  the  imaging  system  are  small  in  comparison  to  measurement  noise  levels.  Eval¬ 
uation  of  motion  and  structure  estimation  schemes  on  simulated  imagery,  however, 
provides  an  efficient  means  to  verify  and  compare  performance  of  the  three  filtering 
algorithms. 

The  geometry  of  Figure  3.1  was  used  to  generate  binocular  image  sequences  of 
a  single,  rigid,  translating  and  rotating  block.  Figure  4.1  shows  the  object  used  for 
simulations  with  the  convention  for  labelling  of  feature  points.  Features  of  interest 
are  four,  p^,  and  p^,  of  the  eight  corners  of  a  rectangular  block  20  units  wide 
(pP  — ♦  p?)  and  10  units  in  height  (p®  — »  p*)  and  depth  (p^  — ♦  p’').  Feature  detection 
and  both  temporal  and  spatial  correspondence  information  is  provided  by  the  imagery 
generation  program,  and  occlusion  is  not  explicitly  considered  in  this  work. 

In  all  simulations  of  this  Chapter,  two  cameras  are  employed  with  unity  focal 
length.  The  position  and  orientation  of  the  left  camera  (Ci)  is  defined  by  a  translation 
[Tf  Jf;  =  [0,  —5,  — 30]£  units,  and  transformation  1^,  formed  from  Euler  angles  ^Ci  = 
10*  (yaw  or  pan),  dci  =  —45“  (pitch  or  tilt),  and  =  0  (roll  about  optical  axis). 
The  position  and  orientation  of  the  right  camera  (Cj)  is  defined,  respectively,  by 
=  (0,5, -30]£  units,  and  formed  from  Euler  angles  =  -10“,  Bc^  = 
—45*,  and  =  0.  Measurement  error  in  feature-based  motion  analysis  studies  is 
often  taken  as  uniform  digitization  noise  [19,  20,  32,  34].  Simulations  of  this  Chapter 
assume  moderately  high  levels  of  measurement  noise  in  the  range  ±0.02  units  in  the 
image  plane,  which  corresponds  to  errors  on  the  order  of  4%  to  8%  of  the  size  of  the 
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Figure  4.1 

Object  used  in  simulation  studies.  Feature  points  of  interest  are 
labelled  p®,  p®,  p®,  and  p'. 

object  in  the  image  when  the  object  is  closest  to  the  cameras.  Measurement  models 
in  the  Kalman  filters  assume  Gaussian  statistics  with  standard  deviation  0.02  units 
in  both  horizontal  and  vertical  components.  As  a  result,  the  measurement  models  are 
not  exactly  matched  to  the  true  statistics  of  measurement  error,  as  would  probably 
be  the  case  in  any  implementation. 

Section  4.1  provides  a  brief  investigation  of  Cramer  Rao  bounds,  first  for  the  reset¬ 
ting  requirement  of  the  angle-axis  filter,  and  second  to  compare  optimal  performance 
for  the  three  filters.  Section  4.2  provides  an  overview  of  simulations  and  results  which 
are  presented  in  detail  in  Section  4.3.  A  closing  summary  and  discussion  is  provided 
in  Section  4.4. 


4.1  Comparison  of  Cramer  Rao  Bounds 

As  discussed  in  Section  2.2,  the  extended  Kalman  filter  is  generally  a  nonlinear, 
suboptimal  state  estimation  technique.  Performance  assessment  of  such  techniques 
often  employ  the  Cramer-Rao  inequality  [79]  to  derive  covariance  lower  bounds.  Ap¬ 
pendix  C  reviews  the  significance  and  derivation  of  Cramer-Rao  lower  bounds  (CRLB) 
for  this  estimation  problem.  Generally,  the  error  covariance  matrix  S(A^)  of  an  un- 
biased  estimator  x{N)  of  the  state  x(A^),  where  the  estimate  is  based  on  the  prior 
information  in  Xo  and  the  observations  z(0),...,z(A^),  is  lower-bounded  by  the  in¬ 
verse  of  Fisher’s  information  matrix  [80,  pp.  91-93).  This  result,  in  theory,  provides 
estimation  error  variance  lower  bounds  for  elements  of  x{N).  It  should  be  noted. 
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however,  that  these  bounds  assume  unbiased  state  estimation;  a  state  dependent  bias 
can  result  in  higher  or  lower  bounds  [78]  (see  Appendix  C).  As  a  result,  most  analysis 
{28]-[31],  [61],  [62],  proceed  under  the  assumption  of  unbiased  estimation  resulting  in 
approximate  covariance  bounds. 

A  further  assumption  in  deriving  Cramer  Rao  bounds  is  that  the  dynamic  model 
is  noise-free.  This  means  that  the  dynamic  model  from  which  the  filter  is  constructed 
is  assumed  to  exactly  match  the  deterministic  temporal  behavior  of  observed  pro¬ 
cesses.  For  the  purposes  of  satisfying  this  assumption  during  investigations  of  this 
Section,  modeling  parameters  a^,  ai,  and  <Tr  are  set  to  zero,  thereby  eliminating 
process  noise  and  reducing  the  dynamic  models  to  truncated  Taylor  series  expansions, 
the  initialization  parameter  vector  do  is  set  to  zero,  which  means  that  the  origin  of 
the  object  centred  reference  frame  lies  at  a  feature  point,  and  true  object  motion  is 
defined  such  that  it  can  be  exactly  modelled  by  any  of  the  three  parameterizations  of 
rotational  motion.  These  restrictions  obviously  place  some  limitations  on  the  overall 
scope  of  evaluations  with  Cramer  Rao  bounds,  however,  comparisons  of  optim2d  per¬ 
formance  possible  under  these  ideal  circumstances  provides  a  general  appreciation  for 
accuracy  that  can  be  expected  for  more  complex  motion  investigated  in  Sections  4.2 
and  4.3. 

It  was  noted  in  Section  3.5.1  that  the  orientation  vector  of  the  angle-axis  filter 
must  be  reset,  first  to  mjuntajn  a  finite  magnitude  (rotation  angle)  when  the  ob¬ 
ject  continually  rotates  in  one  direction,  and  second  to  yield  decreasing  Cramer  Rao 
bounds.  Figure  4.2  shows  Cramer  Rao  bounds  in  the  absence  of  prior  information 
(P"^  =  0)  for  the  rotational  state  vector  Xr  defined  in  (3.41)  for  purely  rotational 
motion  with  =  1,  i.e.  constant  first  time  derivative  of  Here,  object  structure 
is  assumed  to  be  known,  and  is  removed  from  the  state  vector.  The  origin  of  the 
object-centred  frame,  which  lies  at  the  block  centroid,  remains  fixed  at  [30,0, 0]^ 
units,  with  rotational  motion  about  an  axis  parallel  to  Z£  defined  by  ((0)  =  [0, 0, 0]^ 
and  ((0)  =  [0,0, 1]^  raulians/s.  Note  that  in  this  case  the  reset  is  exact  since  the  axis 
of  the  rotation  matrix,  is  constant. 

The  top  two  graphs  of  Figure  4.2  are  shown  without  applying  the  reset  of  (3.45) 
while  the  bottom  two  graphs  are  shown  with  resetting.  Without  the  reset,  and  for 
this  trajectory,  the  CRLB  of  the  z-  and  y-components  of  (  increa^-e  after  2  seconds. 
The  CRLB  of  the  x-  and  y-components  of  (  are  approximately  an  order  of  magnitud  - 
larger  than  the  x-component  CRLB  at  the  end  of  the  trajectory.  Because,  for  this 
trajectory,  =  ^2  =  0,  the  second  and  fourth  terms  in  expression  (D.IO)  for  dV^fdij 
with  j  -  1,2  are  zero.  As  7  =  |[^||  increases,  dV^fd^j  — »  0  for  j  =  1,2  which  implies 
that  the  rotation  matrix  becomes  insensitive  to  chainges  in  and  ^2  which,  in  turn, 
leads  to  increasing  CRLB’s.  The  bottom  two  graphs  of  Figure  4.2  show  CRLB’s  for 
(  and  (  with  resetting  applied  as  shown  in  (3.45).  In  this  case,  the  bounds  exhibit  a 
decreasing  trend  over  the  trajectory  with  final  values  after  20  seconds  of  approximately 
0.005  radians  for  (  and  0.0005  rad'f-ns/s  for  (. 
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Figure  4.2 

CRLB’s  of  orientation  vector  a)  without  and  b)  with  resetting 


Figures  4.3  through  4.6  show  Cramer  Rao  bounds  for  the  three  filters  in  the 
absence  of  prior  information  (Pq  *  ^  0)  during  a  simple  trajectory  with  constant 
axis  of  rotation.  Measurements  of  positions  of  feature  points  and  p^  (see 

Figure  4.1)  are  available  throughout  the  trajectory.  The  initialization  parameter 
vector  do  is  set  to  zero,  point  p^  is  chosen  to  lie  at  the  origin  of  the  object-centred 
reference  frame,  point  p^  lies  on  the  xq-bxxs^  and  point  lies  in  the  xq-Vo  plane.  All 
three  coordinates  of  p‘  are  unknown.  True  motion  of  the  object  is  defined  by  constant 
acceleration  translational  motion  of  point  p^, 

p(0)  =  [30-,-20;0]J  units, 

p(0)  =  (2;2;— 21e  units/s,  and  (4.1) 

p  [-0.4;  0.4;  0.4] J  units/s’, 

and  rotational  motion  about  the  yo*axis  with  constant  angular  acceleration. 
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(4.4) 


The  angle-axis  filter  is  reset  twice  during  this  trajectory  at  about  2.1  and  8.2  seconds, 
and  pitch  in  the  roll-pitch-yaw  filter  passes  through  the  singular  point  of  $  =  t/2  at 
about  1  and  9  seconds.  True  structure  parameters  in  this  case  (measured  in  units) 
are 


■  0  ■ 

■  0  ■ 

■  10  ■ 

0 

,  ro  = 

20 

,  ro  = 

0* 

-10 

0* 

0* 

(4.5) 


where  (•)*  indicates  a  known  vsJue. 

Cramer  Rao  bounds  for  translational  motion  estimation,  Figure  4.3  and  top  of 
Figure  4.4,  demonstrate,  as  might  be  expected  due  to  identical  models  for  trans¬ 
lational  motion,  that  bounds  on  all  three  filters  are  very  close  for  this  trajectory, 
especially  for  the  angle-axis  and  roll-pitch-yaw  filters.  Bounds  for  the  quaternion 
filter  lie  slightly  above  those  of  the  other  two  parameterizations.  Bounds  on  orienta¬ 
tion  parameters,  shown  in  the  lower  part  of  Figure  4.4,  exhibit  oscillatory  behavior, 
but  a  decreasing  trend  over  the  trajectory.  These  oscillations  appear  to  contradict 
the  notion  that  as  more  measurements  become  available,  more  information  should 
be  accumulated  about  the  observed  process,  which  means  that  one  might  expect  the 
bounds  to  decrease  monotonically.  Appendix  C  clarifies  the  meaning  of  these  bounds 
in  the  present  context  and  demonstrates  why  they  need  not  decrease  monotonically. 


Figure  4.5  compares  covariance  bounds  for  estimation  of  angular  velocity  and 
acceleration.  Again  very  similar  behavior  is  demonstrated  for  the  angle-axis  and 
roll-pitch-yaw  filters,  whereas  bounds  for  the  quaternion  filter  are  slightly  higher. 
Figure  4.6  shows  the  bounds  for  estimation  of  the  six  structural  parameters.  Again, 
almost  identical  behavior  is  shown  for  the  angle-axis  and  roll-pitch-yaw  filters.  How¬ 
ever,  significantly  higher  bounds  are  present  for  the  quaternion  filter  in  the  estimation 
of  the  zo-coordinate  of  p’,  the  yo'Coordinate  of  p^,  and  the  Zo'coordinate  of  p^.  Note 
that  these  are  precisely  the  non-zero  parameters  in  the  structure  set  shown  in  (4.5). 
This  investigation  was  repeated  for  different  combinations  of  structural  parameters 
and  always  yielded  similar  results  in  that  quaternion  filter  bounds  for  non-zero  struc¬ 
tural  parameters  were  significantly  above  those  for  the  other  two  filters. 
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Figure  4.3 

Cramer  Rao  bound  comparison  of  position  and  velocity. 
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Figure  4.4 

Cramer  Rao  bound  comparison  of  acceleration  and  orientation. 
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Figure  4.5 

Cramer  Rao  bound  comparison  of  rotational  motion. 
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Figure  4.6 

Cramer  Rao  bound  comparison  of  structure  estimation. 
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4.2  Overview  of  Simulations  and  Results 

SimulatioD  studies  have  been  conducted  over  a  wide  range  of  trajectories,  begin- 
ning  with  known  structure  and  simple  motion,  and  progressing  to  completely  unknown 
structure  with  more  complex  motion.  Various  sets  of  parameters  were  employed  for 
dynamical  models  in  the  filters  during  these  simulations.  A  simple  process  for  select¬ 
ing  physically  justifiable  parameters  led  to  the  best  results. 

Because  measurements  axe  related  directly  only  to  object  position  and  orientation, 
estimation  of  high-order  time  derivatives  often  demonstrated  serious  lag  and  overshoot 
which  can  lead  to  filter  instability.  Consequently,  motion  only  up  to  the  second  time 
derivative  is  included  in  dynamic  models  by  setting  Nt  =  2,  =  2,  =  1,  and 

N(  =  2.  Following  Singer’s  approach  [71]  in  Equation  (3.33)  of  Section  3.4,  the 
manoeuvre  correlation  time  constant  is  estimated  for  the  expected  class  of  objects 
and  motion,  extreme  values  are  estimated  for  the  manoeuvre  variables  of  translational 
and  rotational  motion,  and  probabilities  are  assigned  to  the  occurrence  of  maximum, 
minimum  and  zero  values  of  the  manoeuvre  variable.  Results  shown  in  Section  4.3 
were  generated  assuming  a  manoeuvre  correlation  time  constant  of  2  seconds  which 
gives  at  =  Or  =  0.5s~^.  Extreme  values  of  acceleration  for  translational  motion  is 
estimated  as  =  12  units/s’  which  occur  with  probability  Ptm/a  -  0  .1,  and  zero 

translational  acceleration  occurs  with  probability  Pt,  =0.1.  These  values  in  Equation 
(3.33)  give 


The  continuous-time  white  noise  process  driving  the  translational  motion  model  then 
has  covariance  Qt  =  2at(Tfl3,  and  the  approximation  of  (3.32)  is  employed  to  com¬ 
pute  the  covariance  matrix  of  the  corresponding  discrete-time  process  noise  sequence. 

Corresponding  parameters  for  rotational  motion  are  Mrmax  —  6rad/s’,  Pr _ =0.1, 

and  Pro  =  0.1,  which  gives  a,  «  4  and  Q,  =  2ara^fh.  Integration  of  the  quaternion 
differential  equation  with  N^r  =  5  was  found  to  give  sufficient  accuracy. 

A  subset  of  results  from  three  trajectories  are  presented  in  Section  4.3. 

Case  1  With  model  parameters  selected  as  described  above,  the  Extended  Kalman 
filter  keeps  the  gains  relatively  large  in  order  to  follow  manoeuvres  as  they  oc¬ 
cur.  Consequently,  although  performance  for  manoeuvring  trajectories  is  good, 
performance  for  nonmanoeuvring  trajectories  may  be  degraded  from  that  of  sim¬ 
pler  filters  {71).  This  Case  considers  the  nonmanoeuvring  trajectory  employed 
in  the  coiiipcuison  of  Ci<uiicr  Rao  bounds  in  Section  4.1,  and  shows  results  of 
filters  exactly  “matched”  to  the  trajectory  (no  process  noise)  and  results  from 
“unmatched”  filters  formed  from  the  parameter  selection  above.  Results  demon¬ 
strate  that  although  performance  of  the  unmatched  filters  are  slightly  degraded 
in  the  estimation  of  translational  and  rotational  time  derivatives,  performance 
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in  the  estimation  of  position,  orientation,  and  structure  are  very  similar  for  both 
the  matched  and  unmatched  filters. 

Case  2  True  object  motion  is  defined  with  constant  acceleration  of  the  block  cen¬ 
troid,  and  rotational  motion  computed  from  an  angle-axis  formulation  which 
cannot  be  exactly  matched  by  any  of  the  three  parameterizations  of  rotational 
motion  employed  in  the  filters.  The  origin  of  the  object-centred  reference  frame 
does  not,  due  to  the  initialization  algorithm  described  in  Section  3.6,  lie  at  the 
block  centroid,  and  hence  translational  motion  models  in  each  filter  are  not 
matched  to  the  trajectory.  The  angle-axis  filter  gives  slightly  better  overall  per¬ 
formance  for  this  trajectory  than  the  quaternion  and  roll-pitch-yaw  filters;  the 
roll-pitch-yaw  filter  demonstrates  poorer  performance  in  estimation  of  angular 
velocity,  and  the  quaternion  filter  exhibits  poorer  performance  in  the  estimation 
of  position  and  some  structural  components. 

Case  3  The  trajectory  of  Case  2  was  based  on  an  angle-axis  formulation  in  which 
case  one  might  expect  the  angle-axis  and  quaternion  filters  to  more  accurately 
match  the  trajectory  than  the  roll-pitch-yaw  filter.  The  trajectory  of  Case  3 
defines  true  object  motion  with  constant  acceleration  of  the  block  centroid, 
as  in  Case  2,  but  rotational  motion  is  computed  from  a  sinusoidal  roll-pitch- 
yaw  formulation.  As  in  Case  2,  neither  translational  nor  rotational  dynamic 
models  are  matched  to  the  trajectory.  The  angle-axis  and  roll-pitch-yaw  filters 
show  almost  identical  performance  in  this  case.  Again  the  quaternion  filter 
exhibits  poorer  performance  in  the  estimation  of  position  and  some  structural 
components. 

Results  from  these  three  Cases  are  representative  of  all  results  obtained  from 
vjuious  trajectories  investigated  during  simulation  studies.  In  general,  all  three  filters 
demonstrated  satisfactory  tracking  of  manoeuvring  objects  with  the  angle-axis  filter 
giving  slightly  better  performance  than  the  quaternion  and  roll-pitch-yaw  filters.  The 
quaternion  filter,  apart  from  its  higher  computational  requirements,  appears  to  give 
poorer  (slower)  performance  in  tracking  position  and  estimation  of  structure.  The  roll- 
pitch-yaw  filter,  which  is  the  simplest  from  a  computational  point  of  view,  sometimes 
showed  significant  error  in  angular  velocity  estimates.  The  angle-axis  filter  is  only 
slightly  more  computationally  demanding  than  the  roll-pitch-yaw  filter.  The  following 
Section  presents  detdled  graphical  results  for  the  above  three  cases,  and  provides 
further  discussions  on  relative  performance  of  the  three  filters. 


4.3  Detailed  Simulation  Results 

This  Section  presents  partial  sets  of  simulation  results  for  the  three  Cases  outlined 
in  Section  4.2.  The  vision  system  and  object  geometry,  and  measurement  noise  levels 
are  as  described  in  the  Introduction  to  this  Chapter.  With  four  feature  points  of 
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interest,  the  angle-axis  and  roll-pitch-yaw  filters  have  24  states,  while  the  quaternion 
filter  has  25  states.  Presentation  of  results  for  each  Case  includes  a  discussion  of  the 
true  object  motion,  initialization  parameters,  true  structure  (which  depends  on  the 
initialization  parameter  vector  do),  and  the  resultant  image  sequences.  Graphical 
presentation  may  include  results  of  single- run  samples  and/or  60- run  Monte  Carlo 
simulations.  In  order  to  duplicate  true  motion  and  structure  over  Monte  Carlo  sim¬ 
ulations,  initialization  during  the  first  run  selects  do  (the  same  for  all  three  filters), 
and  subsequent  runs  employ  the  same  do  as  the  first,  but  with  other  parameters 
being  initialized  as  outlined  in  Section  3.6. 

The  angle-axis  and  roll-pitch-yaw  formulations  provide  intuitive  parameterizations 
for  the  three-dimensional  rotational  group.  However,  to  say  that  an  estimated  quater¬ 
nion  q  has  an  error  q,  what  does  this  mean  in  terms  of  the  error  in  the  corresponding 
basis  transformation  =  !£  —  If?  A  convenient  measure  of  estimation  accuracy  at 
the  kth  time  step  is  given  by  the  scaled  Frobenius  norm 

CW  =  5^(Tr{ii(l)(igm))*.  (4.7) 

The  Frobenius  matrix  norm  without  the  normalization  factor  l/{2y/Z)  has  been  em¬ 
ployed  in  investigations  such  as  [65].  Because  l£(ib)  and  l£(ib)  are  both  orthogonal 
for  all  three  parameterizations,  the  scale  factor  employed  in  (4.7)  implies  that  the 
smallest  interval  containing  C{k)  is  [0, 1). 

Angular  velocity  is  avulable  directly  in  the  quaternion  filter,  and  is  computed  from 
state  estimates  using  Equations  (3.40)  and  (3.85)  for  the  angle-axis  and  roll-pitch- 
yaw  filters,  respectively.  Errors  in  angular  velocity  estimation  are  shown  as  sample 
or  root-mean-square  values  of  the  Euclidean  norm  of  the  difference  between  true  and 
estimated  angular  velocity  vectors.  Sample  results  show  true  and  estimated  values  of 
position  and  velocity,  whereas  Monte  Carlo  simulation  results  show  root-mean-square 
values  of  the  Euclidean  norm  of  the  difference  between  true  and  estimated  position 
and  velocity  vectors,  respectively.  Results  for  structure  estimation  show  estimation 
errors  for  sample  runs  and  root-mean-square  values  of  the  Euclidean  norm  of  the 
difference  between  true  and  estimated  structure  vectors  for  Monte  Carlo  simulations. 

Case  1:  This  Case  considers  the  nonmanoeuvring  trajectory  employed  in  the 
comparison  of  Cramer  Rao  bounds  in  Section  4.1,  and  shows  results  of  filters  exactly 
"matched”  to  the  trajectory  (no  process  noise)  and  results  from  "unmatched”  filters 
formed  from  parameter  selection  as  shown  in  Table  I. 

The  matched  filters  set  Ot  =  Or  =  0  s"*,  Oi  =  0  units/s*,  and  <t,  =  0  rad/s*,  while 
the  unmatched  filters  employ  a<  =  o,  =  0.5  s"*,  Oi  =  8  units/s*,  and  o,  =  4  rad/s*. 
The  initialization  parameter  vector  do  is  set  to  zero  for  this  investigation.  True 
translational  motion  of  the  origin  of  the  object-centred  frame,  which  lies  at  feature 
point  pi*,  true  rotational  motion  about  p^,  and  true  structure  parameters  are  given  in 
Table  I  along  with  a  sample  initialization  denoted  by  x«,i,r(0).  Note  that  structure 
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Simulation  parameters  for  Case  1.  Table  entries  replaced  by  ‘NC’ 
indicate  that  quantities  have  not  been  computed  and  are  not  used 
in  initialization. 


Structure:  Nj  —  4,  <t(x,)  =  5.0,  do  =  [0,0,0]^ 


(0,0.10) 

(-4.54, 


[10,do,2,do,3] 

[17.96,do2,<Iojl 


X. 


[0, 20,  dp^] 

[-11.56,18.87,'^;^ 


i.(0) 


Translation:  Nt  =  2y  at  =  0  and  0.5,  at  =  0  and  8 


p 

P 

P 

x.(0) 

[30,-20,0] 

12,2,-21 

[-0.4, 0.4, 0.4] 

x.(0) 

[31.84,-20.72,2.23] 

10,0,01 

5AoI 

^(x«) 

5.0 

5.0 

5.0 

Rotation:  Angle-Axis,  =  2,  Or  =  0  and  0.5,  gy  =  0  and  4 


x.(0) 

[0,-0.7854,0] 

NC 

NC 

^(0) 

[0.0052,-0.72,  -0.2] 

[0,0,0] 

10,0,01 

<y(^) 

0.5 

2.0 

2.0 

Rotation:  Quaternion,  N^,  =  1,  N,  =  5,  Or  =  0  and  0.5,  =  0  and  4  || 

q 

w 

w 

x.(0) 

[0,0.38,0,0.92] 

NC 

NC 

Xr(0) 

[0.0025,0.35,0.098,0.93 

ToaST 

ToTmT 

<^(x,) 

0.5 

2.0 

2.0 

II  Rotation:  Roll-Pitch- Yaw,  N(  =  2,  Or  =  0  and  0.5,  <7r  =  0  and  4  || 

C  =  W,*,,M 

C 

c 

C 

x,(0) 

[0,-0.7854,0] 

NC 

NC 

*r(0) 

[0.084,-0.72,-0.24] 

[0,0,0] 

10,0,0 

<^(Xr) 

0.5 

2.0 

2.0 
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initialization  for  the  sample  shown  has  significant  errors,  particularly  in  The 
initial  filter  covariance  matrices  are  taken  as  diagonal  with  the  square- roots  of  diagonal 
entries  corresponding  to  standard  deviations,  as  indicated  in  Table  I,  of  5  units  for 
structure  states,  5  units  for  position  and  its  time  derivatives,  0.5  for  orientation,  and 
2.0  Rad/s  for  time  derivatives  of  orientation  parameters. 

True  and  noisy  feature  point  trajectories  in  the  image  plane  of  the  left  camera 
are  shown  in  Figure  4.7.  In  the  image,  the  top  of  the  object  first  rotates  towards  the 
observer,  slows  down,  stops,  and  then  reverses  its  direction  of  rotation  to  return  to 
its  original  orientation.  The  axis  of  rotation  is  constant  for  this  trajectory.  Measure¬ 
ment  noise,  illustrated  in  the  lower  half  of  Figure  4.7  is  moderately  high,  but  free  of 
significant  outliers  due  to  the  assumption  of  uniformly  distributed  errors.  In  practice, 
the  frequency  of  occurrence  of  outlier  measurements  for  object  tracking  filters  could 
be  reduced  through  a  hierarchical  approach  similar  to  that  outlined  in  Section  1.2,  or 
rejected  with  chi-squared  tests  as  employed  by  Ayache  and  Faugeras  [40]. 

Figures  4.8  and  4.9  show  60-run  Monte  Carlo  simulation  results  for  matched  and 
unmatched  filters,  respectively.  In  the  absence  of  process  noise.  Figure  4.8,  the 
Kjtlman  filters  eventually  neglect  further  measurements  as  information  is  accumu¬ 
lated.  This  is  demonstrated  by  the  sample  trace  (lower  right  graph  of  Figure  4,8) 
of  the  maximum  principal  value  (singular  value)  of  the  Kalman  gain  matrix  over 
time.  For  this  particular  trajectory  and  measurement  noise,  the  filter  gains  appear 
to  prematurely  reject  further  measurements.  This  is  demonstrated  by  the  increasing 
root-mean-square  errors  in  position  estimation,  with  the  angle-axis  filter  showing  the 
poorest  performance  in  the  second  half  of  the  trajectory.  Cramer  Rao  bounds  for  po¬ 
sition  estimation  at  the  end  of  the  trajectory,  from  Figure  4.3,  are  approximately  0.3 
units  in  each  component,  or  very  roughly  0.3\/3  »  0.5  units  for  the  Euclidean  norm 
of  position  errors.  This  suggests  reasonably  good  performance  of  the  quaternion  and 
roll-pitch-yaw  filters  in  position  estimation.  The  quaternion  filter  shows  slightly  bet¬ 
ter  performance  in  translational  velocity  and  orientation  estimation,  while  all  three 
filters  yield  similar  performance,  in  comparison  to  Cramer  Rao  bounds,  for  angular 
velocity  and  structure  estimation. 

Figure  4.9  shows  results  for  the  three  filters  with  parameters  selected  to  track 
manoeuvring  objects.  All  three  filters  demonstrate  similar  performance  during  this 
simulation.  In  this  case  the  maximum  principal  gains  are  maintained  at  approximately 
95  to  100  after  initial  transients.  In  comparison  to  performance  of  the  matched 
filters  of  Figure  4.8,  position  orientation  and  structure  estimation  is  very  similar,  but 
translational  and  rotational  velocity  estimation  is  degraded.  Generally,  higher  filter 
gains  were  found  to  increase  root-mean-square  errors,  but  decrease  mean  errors  (bias) 
in  velocity  and  higher  time  derivatives.  These  results  demonstrate  the  conflicting 
requirements  of  keeping  the  gains  small  enough  to  suppress  measurement  noise  while 
at  the  same  time  keeping  the  g^ns  high  enough  to  track  manoeuvring  objects  or  to 
maintain  robustness  in  the  presence  of  unmodelled  dynamics. 
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Figure  4.8 

Monte  Carlo  simulation  results  for  Case  1  without  process  noise. 
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Figure  4.9 

Monte  Carlo  simulation  results  for  Case  1  with  process  noise. 
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Case  2:  In  this  case,  true  object  motion  is  defined  with  constant  acceleration  of 
the  block  centroid,  and  rotational  motion  computed  from  an  angle-axis  formulation. 
For  the  purposed  of  describing  the  true  motion  of  the  block,  define  Fs  as  the  block- 
centred  reference  frame  used  in  the  imagery  generation  program.  Note  that  Fb  and 
Fo  are  generally  distinct,  but  for  the  purpose  of  simulation  differ  only  by  a  translation 
(so  that  =13)-  The  origin,  Ob,  of  Fb  lies  at  the  block  centroid,  and  the  coordinate 
planes  of  Fp  are  parallel  to  faces  of  the  block.  Translational  motion  of  Ob  was  defined 
in  the  imagery  generation  program  with 

[T|]b(0)  =  [30,-20,0)1  units, 

[tf]£;(0)  =  [2, 2, -2)1  units/s,  and  (4.8) 

[tflE  =  (-0.4,0.4,0.411  units/s*. 

Recall  that  the  initialization  routine  computes  do,  which  defines  the  position  of  the 
origin  of  the  object  centred  frame,  based  on  observations  during  the  first  measurement 
event.  The  origin  of  the  object-centred  frame  employed  in  the  filters,  therefore,  will 
not  generally  coincide  with  the  centroid  of  the  block  and,  due  to  rotational  motion, 
will  not  have  the  constant  acceleration  motion  defined  in  (4.8).  Specifically,  the  filters 
are  required  to  estimate  translational  motion  of  Fq  whose  position  and  velocity,  for 
example,  are  given  by 

P«)  =  |Xg|E(<)  =  [TijE(<)  +  lil?  {r?  -  Idol*}  ,  (4-9) 

m  =  Itil*  +  Iwgl'^Igll  {rg  -  Ido)*} ,  (4.10) 

where  the  velocity  equation  uses  (3.35)  and  the  fact  that  is  constant.  Higher- 
order  time  derivatives  of  true  translational  motion  are  computed  in  a  strzught-forward 
manner  by  differentiating  (4.10). 

Rotational  motion  of  the  block-centred  reference  frame  is  defined  in  the  imagery 
generation  program  with  an  angle-axis  parameterization, 

If(f)  =  exp(»/(0u*(<))-  (4.11) 

The  angle  of  rotation,  has  constant  second  time  derivative, 

»j(0)  =  0  rad,  ^(0)  =  0.4x  rad/s,  and  rj  =  — 0.08x  rad/s*.  (4.12) 

Temporal  behavior  of  u,  since  it  is  a  unit  vector,  must  be  rotational  with  instantaneous 
angular  velocity,  say,  w„(<)  in  the  earth-fixed  frame  so  that 

d(0  =  wi(<)u,  (4.13) 

and  the  initial  condition  is  specified  as 

0(0)  =  ^[1,  1,  1]’’.  (4.14) 
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The  angular  velocity,  Wu,  of  the  axis  of  the  basis  transformation  (this  is  not  the 
angular  velocity  of  the  object)  is  defined  with  constant  second  time  derivative, 


■-0.2' 

-o.r 

0.02  ■ 

II 

o' 

3 

-0.2 

0.2 

rad. 

w„(0)  = 

-0.1 

0.1 

rad/s. 

w„  = 

0.02 

-0.02 

rad/s*. 


(4.15) 


Integration  of  (4.13)  is  performed  numerically  using  a  method  similar  in  form  to  the 
quaternion  integration  scheme  described  in  Section  3.5.2  by  defining  a  partition  of 
each  sample  period,  assuming  w«  is  approximately  constant  over  each  interval  of  the 
partition,  and  using  the  closed  form  expression  for  the  matrix  exponential  of  a  3  x  3 
skew  symmetric  matrix  given  in  (3.39).  As  in  the  quaternion  integration  scheme,  this 
method  ensures  that  u{t)  remains  a  unit  vector. 

Table  II  summarizes  simulation  parameters  for  this  Case  and  provides  numerical 
values  of  a  sample  initialization.  A  subsequence  of  images  viewed  in  the  left  camera 
is  shown  in  Figure  4.10.  In  this  Figure,  each  frame  sample  number  is  shown  in  the 
lower  left;  the  sequence  proceeds  across  the  top  row  from  left  to  right,  back  across  the 
second  row  from  right  to  left,  etc.  The  top  of  the  object  first  begins  to  rotate  towards 
the  observer  as  does  the  right  side  of  the  object.  The  object  eventually  returns  to  its 
original  orientation  at  the  end  of  the  trajectory.  True  and  sample  noisy  feature  point 
trajectories  in  the  image  plane  of  the  left  camera  are  shown  in  Figure  4.11.  Note  the 
tight  loops  which  would  have  to  be  tracked  by  temporal  correspondence  algorithms 
such  as  two  dimensional  feature  point  trackers  in  a  hierarchical  approach  similar  to 
that  proposed  in  Section  1.2. 

Sample  results  shown  in  Figures  4.12  and  4.13  correspond  to  the  initialization 
listed  in  Table  II.  Figure  4.12  shows  true  and  estimated  position  and  velocity.  Accu¬ 
rate  tracking  of  position  is  provided  by  all  three  filters,  however,  velocity  estimation 
indicates  slight  lag  and  overshoot,  particularly  in  the  ^-component.  Again  the  maxi¬ 
mum  principal  gain  is  maintjuned  in  the  interval  90  to  100  during  the  second  half  of 
the  trajectory.  Figure  4.13  shows  sample  results  for  orientation,  angular  velocity,  and 
structure  estimation.  Similar  behavior  of  the  three  filters  is  shown  orientation  estima¬ 
tion  with  errors  below  5%  after  initial  transients.  Both  the  angle-axis  and  quaternion 
filter  track  angular  velocity  with  errors  less  than  0.5  rad/s,  but  the  roll- pitch-yaw  filter 
shows  a  fairly  signific2uit  spike  at  approximately  2.5  seconds.  All  structure  estimates 
show  convergence  near  the  end  of  the  trajectory. 

Figure  4.14  shows  root-mean-square  errors  from  a  60-run  Monte  Carlo  simulation. 
Again  very  similar  performance  is  demonstrated  by  all  three  filters,  except  for  slightly 
degraded  estimation  of  position  and  p*  structure  from  the  quaternion  filter.  Note  the 
fairly  significant  oscillations  in  translational  velocity  estimation  due  to  lag  and  some 
overshoot  in  filter  estimates.  The  amplitude  of  oscillation  appears  to  decrease  during 
the  second  half  of  the  trajectory.  Angular  velocity  estimates  from  the  roll-pitch- 
yaw  filter  shows  distinct  peaks  in  root-mean-squared  errors  at  about  2.5,  4.5,  and  6 
seconds.  The  true  pitch  angle  (not  shown)  for  this  trajectory  behaves  like  a  damped 
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Table  II 

Simulation  parameters  for  Cases  2  and  3.  Table  entries  replaced  by 
‘NC’  indicate  that  quantities  have  not  been  computed  and  are  not 
used  in  initialization. 


Structure:  Nf  =  4,  <r(x,)  =  5.0,  do  =  [-3.29,  -4.74,  -3.06]^ 
I  3  I  3 - 1 - rr - 


-5 - 

ro 

x.(0) 

[-3^28,  -4.74,6.94 

[-3.29,15.26,^03] 

[6.71,do.j,do^] 

X, 

t-3.12, -4.80, 6.58 

(-4.37, 16.05,  do.3] 

[9.94,  do  .2,  do  .3] 

Translation:  TV*  =  2,  a*  =  0.5,  =  8  || 

P 

P 

P 

X*(0) 

[28.29,-25.26,-1.94] 

NC 

NC 

X,(0) 

[28.39,  -25.70,  -2.05) 

5;w 

saot 

<^(x«) 

5.0 

5.0 

5.0 

Rotation:  Angle- Axis,  N(  =  2,  or,  =  0.5,  <Tr  =  4  }| 

e 

X,(0) 

(0,0,01 

NC 

NC 

x,(0) 

[-0.048,-0.047,-0.040] 

10,0,0) 

[0,0,01 

<^(Xr) 

0.5 

2.0 

2.0 

II  Rotation:  Quaternion,  =  1,  =  5,  Or  =  0.5,  a,  =  4 

q 

w 

w 

Xr(0) 

[0,0, 0,1] 

NC 

NC 

*.(0) 

[0.024,0.023,0.02,0.9991 

10,0.0] 

0,0,0 

‘y(Xr) 

0.5 

2.0 

2.0 

Rotation:  Roll- Pitch- Yaw,  =  2,  a,  =  0.5,  or,  =  4  || 

II 

C 

c 

C 

x,(0) 

■pAOl 

NC 

NC 

Xr(0) 

[-0.047,0.048,-0.039] 

0,0,0] 

[0,0,01 

<^(Xr) 

0.5 

2.0 

2.0 
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sinusoid  with  peaks  just  prior  to  these  times,  and  has  a  maximum  value  of  about 
0.4t.  Although  estimation  of  pitch  angle  was  very  accurate,  estimation  of  the  time 
derivative  of  pitch,  which  is  also  a  damped  sinusoid,  shows  some  lag  and  overshoot 
much  like  those  occurring  in  translational  velocity  estimation.  This  behavior  would 
contribute  significantly  to  the  peaks  present  in  angular  velocity  estimation  error.  The 
angle-axis  filter  gives  marginally  better  performance  than  the  other  two  filters  for  this 
simulation.  Structure  root-mean-square  estimation  errors  decrease  rapidly  during  the 
first  1  or  2  seconds,  and  appear  to  have  levelled  off  at  about  0.3  to  0.5  units  at  the 
end  of  the  trajectory. 

As  discussed  in  Section  3.2,  there  are  four  possible  orientations  for  the  object- 
centred  reference  frame  with  respect  to  the  three  specicil  feature  points  selected  at 
initialization.  When  structure  was  initialized  with  all  zeros,  for  example,  convergence 
of  structure  estimates  was  observed  in  one  of  the  four  possible  orientations  of  the 
object-centred  frame,  the  particular  one  of  the  four  being  de{>endent  on  initial  filter 
transients.  In  simulations  with  this  object  and  the  given  measurement  noise  levels 
it  was  found  that  the  simple  single-frame  initialization  of  Section  3.6  provided  suffi¬ 
cient  initial  accuracy  for  structure  to  converge  in  the  object-centred  frame  selected 
at  initialization,  although  obviously  this  need  not  always  be  the  case.  Figure  4.15 
shows  the  reliable  convergence  of  structure  estimates  to  true  structural  parameters 
given  in  Table  II  adter  10  seconds  (100  measurement  events)  for  each  of  the  Monte- 
Carlo  simulation  runs.  Again  all  three  filters  give  similar  results,  with  final  estimates 
from  the  quaternion  filter  being  slightly  noisier  than  those  from  the  angle-axis  and 
roll-pitch-yaw  filters. 
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Figure  4,10 

Image  subsequence  from  the  left  camera  for  Case  2.  Image  frame 
numbers  are  shown  in  the  lower  left  corner  of  each  image.  Four 
feature  points,  p\  p®,  and  p^,  are  labelled  in  each  image. 
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Figure  4.11 

True  and  sample  noisy  image  feature  point  trajectories  in  the  left 
camera  without  occlusion  for  Case  2. 
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Figure  4.15 

Final  structure  estimates  in  Monte  Carlo  simulations  for  Case  2. 
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Case  3:  This  trajectory  defines  true  translational  motion  with  constant  acceler¬ 
ation  of  the  block  centroid,  as  in  Case  2,  but  rotational  motion  is  computed  from  a 
sinusoidal  roll-pitch-yaw  formulation.  Again,  let  Fb  denote  the  block-centred  refer¬ 
ence  frame  used  in  the  imagery  generation  program  with  =  I3.  The  origin,  Ob,  of 
Fb  lies  at  the  block  centroid,  and  the  coordinate  planes  of  Fb  are  parallel  to  faces  of 
the  block. 

As  in  Case  2,  translational  motion  of  Ob  was  defined  with 
[TilE(O)  =  [30,-20,01^  units, 

[Tf]£(0)  =  [2,2,-2]£  units/s,  and  (4.16) 

[Tflf;  =  [— 0.4,0.4,0.4]]g  units/s^. 

However,  translational  motion  of  the  object-centred  frame  is  significantly  different 
from  that  of  Case  2  due  to  2Jternate  rotational  motion  of  Fb  about  Ob  which  is 
defined  with  sinusoidal  variation  of  roll  and  pitch  angles  and  linear  yaw  angle  with 
time: 

=  a  s\n{bt) 

6{i)  =  a[l  —  cos(5<)]  (4.17) 

V^(t)  =  6t,  (4.18) 

where 

a=—  and  6=-—.  (4.19) 

6  10  ^  ’ 

Initialization  parameters  in  this  Case  are  identical  to  those  for  Case  2  shown  in 
Table  II.  Figure  4.16  shows  a  subsequence  of  images  from  the  left  camera  for  this 
trajectory.  The  top  of  the  object  containing  feature  points  3  and  7  begins  to  rotate 
towards  the  observer  (increasing  pitch  angle),  while  the  right  side  containing  points  1 
ajid  3  appears  to  move  down  in  the  image  due  to  positive  yaw  and  roll  angles.  Again, 
the  object  returns  to  its  original  orientation  at  the  end  of  the  trajectory. 

Sample  results  for  initialization  parameters  given  in  Table  II  are  shown  in  Fig¬ 
ures  4.17  and  4.18.  All  three  filters  reliably  track  object  position,  but  with  noisier 
results  for  z-velocity  than  shown  in  Case  2.  Almost  identical  velocity  results  are  ob¬ 
tained  from  the  three  filters  and,  although  noisy,  do  not  show  significant  lag  present 
in  results  for  Case  2.  The  maximum  gain  strength,  as  in  Case  2,  is  again  maintained 
between  90  and  100  after  initial  transients.  Very  similar  behavior  between  results  for 
this  trajectory  and  that  of  Case  2  is  shown  in  Figure  4.18  for  orientation  and  angular 
velocity  estimation.  Some  bias,  however,  appears  in  structure  estimates,  particularly 
for  the  quaternion  Alter  in  x-  and  z-coordinates  and  y-coordinate. 

Figure  4.19  shows  Monte  Carlo  simulation  results  for  this  trajectory.  As  in  Case  2, 
the  quaternion  Alter  gives  slightly  poorer  performance  in  estimation  of  position  and 
some  structural  components.  Velocity  root-mean-square  errors  lie  below  2  units/s 
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during  the  second  half  of  the  trajectory  and  do  not  show  significant  oscillations  that 
were  present  in  results  of  Case  2.  Orientation  errors  lie  below  5%  after  about  2 
seconds,  with  final  errors  of  about  2.5%  for  all  three  filters,  which  is  very  similar  to 
results  of  Case  2.  Angular  velocity  errors  decrease  rapidly  during  the  first  1  second 
and  remain  below  about  3  rad/s  for  the  remainder  of  the  trajectory.  Final  root-mean- 
square  errors  for  structure  estimation  are  on  the  order  of  0.5  to  1  unit. 
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Figure  4.16 

Image  subsequence  from  the  left  camera  for  Case  3.  Image  frame 
numbers  are  shown  in  the  lower  left  corner  of  each  image.  Four 
feature  points,  p*,  p^,  p®,  and  p^,  are  labelled  in  each  image. 
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Figure  4.17 

Sample  results  for  Case  3,  position,  velocity  and  gain  strength. 
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Figure  4.18 

Sample  results  for  Case  3,  orientation,  angular  velocity  and  struc¬ 
ture. 
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4.4  Chapter  Summary 

This  Chapter  has  investigated  approximate  Cramer  Rao  bounds  for  a  simple  tra¬ 
jectory,  discussed  parameter  selection  for  dynamic  models,  and  presented  a  reduced 
set  of  simulation  results  which  are  representative  of  performance  achieved  for  a  wide 
range  of  trajectories.  Cramer  Rao  bounds  suggest  very  similar  optimal  performance 
for  the  three  filters  over  a  simple  trajectory,  and  this  is  supported  by  sample  and 
Monte  Carlo  simulation  results  for  more  complex  trajectories  presented  in  Section  4.3. 
One  significant  advaintage  of  employing  dynamic  models  developed  in  Chapter  3  is 
that  model  parameters  can  be  selected  as  proposed  by  Singer  [71]  based  to  a  large  ex¬ 
tent  on  the  expected  class  of  objects  or  motion,  rather  than  purely  through  heuristic 
methods. 

All  three  filters  demonstrated  good  performance  in  tracking  position  and  orienta¬ 
tion  of  manoeuvring  objects,  however,  estimation  of  translational  velocity  can  exhibit 
filter  lag,  as  in  the  trajectory  of  Case  2.  Lag  in  the  filters  could  be  reduced  to  a 
certjun  extent  by  increasing  the  strength  of  filter  gains  (by  increasing  at)  l>ut  this 
may  be  in  conflict  with  required  measurement  noise  suppression.  Angular  velocity 
root-mean-squared  errors  for  Cases  1  and  2  decrease  very  rapidly  during  the  first  10 
measurement  events,  and  thereafter  appear  to  remiun  at  levels  of  about  0.3  rad/s 
(w  17  Deg/s).  In  Case  2,  angular  velocity  estimates  from  the  roll-pitch-yaw  filter 
showed  distinct  peaks  in  root-mean-squared  errors  which  are  due  in  part  to  some  lag 
and  overshoot  in  estimation  of  time  derivatives  of  a  sinusoidal  pitch  angle.  Similar 
behavior,  only  on  a  smaller  scale,  is  seen  in  Monte  Carlo  simulation  results  for  Case  2 
(see  Figure  4.19)  with  a  slight  increase  in  angular  velocity  root-mean-squared  errors 
just  prior  to  5  seconds.  Generally,  performance  of  the  extended  Kalman  filters  was 
degraded  in  the  estimation  of  time  derivatives  of  sinusoidally  varying  states. 

Structure  estimation  for  all  three  filters  showed  a  sharp  decrease  in  root-mean- 
squared  errors  during  the  first  1  or  2  seconds,  with  final  accuracy  after  100  obser¬ 
vations  of  about  0.25  to  0.5  units.  Structure  estimates  for  the  special  feature  point 
(p^  for  these  simulations)  for  which  the  filters  must  estimate  only  a  single  component 
showed  rapid  convergence  to  within  about  0.5  units  after  only  10  to  20  measurement 
events,  while  those  for  the  feature  point  (p^  here)  in  which  two  components  must  be 
estimated  required  about  50  measurement  events. 
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5.  Conclusions  and  Future  Work 


Many  existing  methods  for  motion  and  structure  estimation  proposed  in  the  lit¬ 
erature  have  been  developed  and  examined  for  specific  cases  of  translational  and 
rotational  motion.  Violation  of  these  assumptions  often  results  in  unobservability 
due  to  strict  dependence  of  structural  models  on  the  nature  of  assumed  motion  of  the 
object.  The  planar  motion,  constant  velocity  case  of  Chapter  2  falls  into  this  category 
of  strict  motion  assumptions,  but  provided  a  simplified  framework  to  introduce  the 
Kalman  filtering  approach  while  at  the  same  time  treating  an  important  problem  in 
motion  analysis.  The  planar  motion  filter  has  demonstrated  good  i>erformance  over  a 
wide  range  of  trajectories  with  estimation  accuracy  in  all  states  approaching  Cramer 
Rao  bounds. 

Methods  for  more  general  motion  proposed  and  evaluated  in  this  report  focus 
in  part  on  removing  restrictive  assumptions  concerning  the  nature  of  object  motion 
by  developing  motion,  structure,  and  measurement  models  for  a  manoeuvring  object 
which  is  observed  with  a  multiple-camera  imapng  system.  Strict  assumptions  were 
not  imposed  on  the  mode  of  translational  or  rotational  motion  except  for  "smooth¬ 
ness”  conditions  in  the  sense  that  parameters  can  be  differentiated  with  respect  to 
time.  Object  manoeuvres,  being  "smooth”  and  time  correlated,  are  modelled  as  first- 
order  Gauss- Markov  processes  for  both  translational  and  rotational  motion.  The 
structural  model  has  been  defined  on  the  basis  of  observed  feature  points  only  and 
is  independent  of  object  motion.  In  this  case,  three  or  more  feature  points  observed 
by  two  or  more  cameras  over  multiple  frames  give  sufficient  information  to  estimate 
both  motion  and  structure  of  the  manoeuvring  object. 

Translational  motion  models  were  identical  for  all  three  filters.  Good  position 
tracking  performance  was  demonstrated  by  the  angle- axis  and  roll-pitch-yaw  filters, 
with  only  slight  degradation  of  position  estimation  shown  by  the  quaternion  filter.  If 
the  object  is  undergoing  rotational  motion,  it  is  unlikely  that  the  origin  of  the  object- 
centred  frame  will  coincide  with  the  centre  of  rotation.  As  a  result,  translational 
motion  in  the  presence  of  rotation  will  probably  have  strong  sinusoidal  component)" 
Estimation  of  tcmpoid!  derivatives  of  position  in  this  case  can  exhibit  lag  and  over¬ 
shoot  which  led  to  the  poor  performance  in  velocity  estimation  shown  in  Case  2. 
This  result  demonstrates  the  conflicting  requirements  of  maintaining  large  enough 
gains  to  track  manoeuvring  objects  while  at  the  same  time  keeping  the  gains  small 
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enough  to  suppress  measurement  noise.  Gain  adjustment  is  easily  accomplished,  how¬ 
ever,  through  selection  of  the  manoeuvre  correlation  time  constants  and  estimation 
of  variances  of  the  manoeuvre  variables. 

Unit  quaternions  have  been  examined  by  many  authors  in  the  parameterization 
of  rotational  motion.  Imposing  a  unit  norm  constraJnt  on  the  estimated  quaternion, 
however,  is  not  easily  incorporated  into  observers  such  as  the  extended  Kalman  filter. 
Applying  impulsive  normalization  of  the  estimated  quaternion  following  each  mea¬ 
surement  update — a  common  proposed  intervention  in  quaternion-based  observers — 
was  found  to  contribute  significantly  to  filter  instability.  Instead,  the  quaternion  filter 
is  initialized  with  a  unit  quaternion,  and  propagates  the  quaternion  estimate  over  time 
with  constant  norm.  A  unit  quaternion  and  appropriately  scaled  structure  vectors 
are  then  extracted  as  output  variables  from  the  state  estimate.  Note,  however,  that 
a  continuum  of  valid  solutions  exist  for  quaternion  filter;  the  squared  norm  of  the 
quaternion  estimate  maintained  within  the  filter  is  free  to  vary  inversely  with  mag¬ 
nitudes  of  estimated  structure  vectors.  In  simulations,  the  quaternion  filter  showed 
slightly  degraded  performance  in  the  estimation  of  position  and  some  structure  pa¬ 
rameters  in  comparison  to  the  roll-pitch-yaw  and  angle-axis  filters. 

The  roll-pitch-yaw  parameterization  has  previously  been  reported  to  be  poorly 
behaved,  and  can  lead  to  computationally  demanding  implementations.  The  approach 
taken  here  led,  instead,  to  a  very  simple  filter  which  may  perform  as  well  as  the 
quaternion  and  angle-axis  filters  over  some  trajectories.  However,  some  difficulties 
were  encountered  in  the  estimation  of  angular  velocity  for  the  trajectory  of  Case  2. 
This  may  have  been  due  in  part  to  sinusoidal  temporal  behavior  of  the  true  pitch 
angle  since  it  was  generally  observed,  as  in  the  case  of  translational  motion,  that 
the  extended  Kalman  filter  performed  poorly  in  the  estimation  of  time  derivatives  of 
sinusoidally  varying  states. 

Published  results  using  the  angle-axis  parameterization  in  recursive  motion  and 
structure  estimation  are  not  available  in  the  literature.  Modelling  rotational  motion 
as  a  first-order  Gauss-Markov  process  in  the  orientation  vector  and  its  time  deriva¬ 
tives  led  not  only  to  a  simple  implementation,  but  also  performance  which  was  as 
least  as  good  as  the  other  two  parameterizations  for  all  trajectories  considered.  Al¬ 
though  this  filter  has  very  efficient  time  propagation  due  to  the  linear  dynamic  model, 
computation  of  the  measurement  model  Jacobian  is  slightly  more  complex  than  the 
other  two,  and  an  occasional  reset  of  the  orientation  vector  and  its  time  derivatives 
is  required  to  maintain  the  rotation  angle  to  within  ±7. 

In  all  three  filters,  there  are  four  possible  orientations  for  the  object-centred  ref¬ 
erence  frame  with  respect  to  the  three  special  feature  points  selected  at  initialization. 
When  structure  was  initialized  with  all  zeros,  for  example,  convergence  of  structure 
estimates  was  observed  in  one  of  the  four  possible  orientations  of  the  object-centred 
frame,  the  particular  one  of  the  four  being  dependent  on  initial  filter  transients.  In 
simulations  with  this  object  and  the  given  measurement  noise  levels  it  was  found  that 
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the  simple  single-frame  initialization  provided  sufficient  initial  accuracy  for  structure 
to  converge  in  the  object-centred  frame  selected  at  initialization,  although  this  need 
not  always  be  the  case.  Reliable  convergenc.e  of  structure  estimates  to  true  structural 
parameters  was  observed  during  all  simulations,  with  the  quaternion  filter  giving 
slightly  noisier  results  that  the  angle-axis  and  roll-pitch-yaw  filters.  All  structural 
parameters  are  modelled  as  constants  and  hence  are  not  driven  by  process  noise.  As 
a  result,  the  filter  g^ns  for  structure  states  eventually  become  sm^lll  as  information 
is  accumulated  in  the  filter.  Decoupling  of  structure  states  from  the  filters  is  possible 
and  recommended  after  predefined  confidence  levels  have  been  attaiined. 

Several  important  factors,  such  as  the  expected  nature  of  object  motion,  compu¬ 
tational  complexity,  and  required  accuracy,  must  be  considered  when  selecting  a  pa¬ 
rameterization  of  rotational  motion  for  any  particular  application.  Prior  information 
concerning  motion  constraints,  for  example  the  planar  motion  problem  of  Chapter  2, 
may  clearly  indicate  appropriate  and  often  simplified  parameterizations  and  dynamic 
models.  For  more  general  motion,  however,  results  of  this  investigation  suggest  that 
the  angle-axis  filter  may  provide  a  computationally  efficient  and  sufficiently  accurate 
means  to  recover  both  structure  and  motion  of  a  manoeuvring  object. 

Research  leading  to  this  report  has  focused  on  only  a  single  component  of  the 
overall  motion  analysis  problem.  Future  work  will  investigate  further  components  of 
this  problem  in  the  context  of  a  hierarchical  structure  such  as  that  proposed  in  Sec¬ 
tion  1.2.  Generally,  the  proposed  system  is  composed  of  three  coupled  multi-target 
tracking  systems  with  increasing  levels  of  complexity;  multiple  features  are  tracked 
in  the  image  planes;  multiple  features  are  tracked  in  three-dimensions;  and  multi¬ 
ple  objects,  composed  of  multiple  features,  are  tracked  in  three  dimensions  with  six 
degrees-of-freedom.  In  such  a  system,  occlusion,  and  temporal  and  spatial  correspon¬ 
dence  as  well  as  rigid  object  segmentation  represent  challenging  problems  which  have 
received  little  attention  in  the  literature. 

Another  primary  consideration  in  future  work  involves  further  evaluation  of  the  ex¬ 
tended  Kalman  filtering  approach  in  comparison  to  other  nonlinear  observers.  Adap¬ 
tive  extended  Kalman  filtering,  Lyapunov  methods,  transformation  to  nonlinear  ob¬ 
server  canonical  form,  nonlinear  map  inversion,  and  sliding  mode  observers  are  re];>- 
resentative  of  recently  proposed  methods  which  may  lead  to  more  robust  observers 
for  recovery  of  object  motion  and  structure  from  multiple-camera  image  sequences. 
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Appendix  A 

Parameterizations  of  Relative 
Orientation 


Chapters  2  and  3  introduced  notation  and  defined  a  change-of- basis  transformation 
such  that  a  vector  Tq  in  Fo  is  transformed  to  [ro]E,  its  representation  in  Fe, 
through  the  expression 

Nb  =  l£ro.  (A.l) 

As  before,  the  object-centred  frame,  Fq,  is  defined  by  a  point  0o>  the  origin  of  Fq, 
and  a  right-handed,  ordered,  orthonormal  basis  set  {io  Jotko}-  Equivalent  notation 
also  applies  to  the  earth-fixed  frame  Fg.  In  order  to  focus  on  relative  orientation 
only,  it  is  assumed  throughout  this  appendix  that  Oo  &nd  Ob  coincedent.  In  this 
case, 

(rols  =  te  (A.2) 

since  the  translation  vector  from  Fe  to  Fo  is  zero.  The  orthogonal  transformation 
belongs  to  the  three-dimensional  rotation  group,  denoted  here  by  7^3,  for  which  there 
are  about  eight  commonly  used  representations  [67].  This  appendix  provides  a  tutoriaJ 
on  three  such  representations:  the  Euler  angle-axis,  quaternion  (sometimes  called 
Euler  parameters,  or  Rxxlriquez-Hamilton  parameters),  and  roll-pitch-yaw  (sometimes 
called  Euler  angles)  parameterizations.  The  material  presented  in  this  appendix  draws 
primarily  from  references  [67]-[70],  [82]-[84]. 

The  problem  of  parameterizing  the  group  has  been  of  interest  since  1776, 
when  Euler  first  showed  that  this  group  is  itself  a  three-dimensional  manifold.  Euler 
observed  that  the  general  displacement  of  a  rigid  body  with  one  fixed  point  is  a 
rotation  about  an  axis  through  that  point  (Euler’s  theorem).  In  the  present  context, 
Euler’s  theorem  states  that  Fo  is  related  to  Fe  by  a  rigid  rotation  of  IR^  about  an 
axis  through  their  common  origin.  Since  'Ra  is  known  to  be  three-dimensional,  the 
rotational  degrees-of-freedom  in  IR^  are  at  most  three.  However,  it  is  topologically 
impossible  to  have  a  global  and  nonsingular  three-dimensional  parameterization  of  7^3, 
[67].  In  fact,  Hopf  showed  in  1940  that  the  minimum  number  of  parameters  required 
to  represent  Tlz  in  a  one-to-one  global  manner  is  five,  (see  [67]).  The  quaternion 
representation,  a  four-dimensional  parameterization  of  7^3,  results  in  a  two-to-one 
mapping  of  IR^ onto  7^3,  and  is  often  considered  sufficient  for  most  practical  purposes. 
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It  is  important  to  appreciate  a  rather  subtle  distinction  between  two  interpreta¬ 
tions  of  operations  performed  by  elements  of  'R.^-  One  interpretation  treats  an  element 
R  €  ^3  as  a  rotation  operator  which  relates  two  vectors,  say  x  and  y  with  llx||  =  |Iy|l, 
in  the  same  bases  according  to  y  =  Rx.  In  this  case,  R  rotates  the  vector  x  into  a 
new  vector  y.  The  second  interpretation  treats  an  element  If  6  "Ra  as  an  orthogonal 
(or  identity,  hence  the  use  of  ‘I’)  change-of-basis  transformation  which  operates  on  a 
vector  expressed  with  respect  to  Fp  to  obtain  a  representation  for  the  same  vector, 
To,  expressed  with  respect  to  Fa  according  to  r^,  =  Ifr/j.  The  main  point  is  that 
vectors  and  To  represent  the  same  physical  entity,  while  the  vectors  x  and  y  in 
general  do  not.  It  is  often  the  case  that  “rotation”  is  used  interchangeably  to  im¬ 
ply  either  interpretation.  This  appendix  deals  exclusively  with  the  “change-of- bases 
transformation”  interpretation. 

This  review  first  introduces  fundamental  properties  of  rotation  matrices  in  Sec¬ 
tion  A.l,  treats  the  Euler  angle-axis  parameterization  in  Section  A.2,  quaternions  in 
Section  A. 3,  and  the  roll-pitch- yaw  representation  in  Section  A. 4. 


A.l  Fundamental  Properties  of 

The  following  list  states  some  fundamental  and  well-known  properties  of  the  ma¬ 
trix  representing  the  change-of- bases  transformation  from  Fo  to  Fb- 

Property  1  The  matrix  Ig  is  orthogonal  with  determinant  -f-1. 

Property  2  The  matrix  has  eigenvalues  {-1-1, e*^}.  Euler’s  axis  of  rotation  of 
is  parallel  to  the  (normalized)  eigenvector,  corresp>onding  to  the  eigenvalue 
-1-1,  while  the  angle  of  rotation,  7,  about  (  is  measured  with  positive  sense  from 
Fe  to  Fo  according  to  the  right-hand-rule  about 

Property  3  The  matrix  can  be  written  as 

=  [  1*o]e  Ijolfi  I^oIb  ] 

“  [  P^^lo  ]  >  (A.3) 

where  superscript ‘T’  denotes  transposition. 

Property  4  The  angular  velocity  of  Fo  with  respect  to  Fe  expressed  in  Fe,  denoted 
[wrIc,  can  be  computed  by  noting  that 

Iw£1e  X  IjoIe,  (A.4) 

(wflE  X  {ko]E, 


d(io]E 

d< 

d[jo]E 

d< 

d(ko]E 

df 
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which  gives 


(A.5) 


where  (•]”  is  the  matrix  cross-product  operator  defined  in  terms  of  components 
of  a  vector  (  =  [6,6,6]^  as 


0  — ^3  ^2 

6  0  -6 
— ^2  6  0 


Rearranging  (A.5)  gives 


(A.6) 


(A.7) 


The  matrix  (tensor)  is  basis-depondent  and  is  transformed  to  Fo  accord¬ 

ing  to 


I^eIo  —  ^(^eIe^e 

=  Igiglgl?  (A.8) 

=  i|ig 

Property  1  follows  from  the  fact  that  both  Fo  and  Fe  are  right-handed  or¬ 
thonormal  triads.  Property  2  then  follows  immediately  from  Property  1.  Property 
3  is  derived  by  considering  identities  of  the  form  [IoIe  =  lE[k?]o  and  noting  that 
[io]o  =  [liO,0]^.  Propierty  4  is  fundamental  in  derivations  of  relationships  between 
angulu  velocity  and  the  selected  parameter  set  and  their  time  derivatives. 


A.2  Euler  Angle-Axb  Parameterization 

According  to  Euler’s  theorem,  a  rippd  rotation  ofSPis  completely  defined  by  a  unit 
vector  (  which  spiecifies  the  axis  of  the  basis  transformation  opierator  and  an  angle 
7  through  which  the  coordinate  system  is  rotated  about  This  is  a  four-parameter 
representation  with  one  quadratic  constraint.  Note  that  (  is  invariant  under  the  basis 
transformation  and  hence  the  subscript  spiecifying  the  frame  to  which  (  is  referenced 
has  been  droppied. 

Given  an  angle  7  and  axis  (  the  corresponding  matrix  can  be  determined  by 
the  following  argument.  Suppxise  at  t  =  0  reference  frames  Fo  and  Fe  are  aligned 
so  that  l£(t  =  0)  =  I3,  the  3x3  identity  matrix.  At  time  t  =  1,  the  orientation  of 
reference  frame  Fo  relative  to  Fe  is  given  by  a  rotation  about  (  in  Fe  through  an 
angle  7.  For  the  purposes  of  deriving  Ig(t  =  1),  the  motion  during  this  time  interval 
can  be  modelled  with  constant  angular  velocity  [wfje  =  7{.  Integration  of  (A.5) 
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from  <  =  0  to  <  =  1  with  initial  condition  I^(f  =  0)  =  I3  immediately  gives  the 
matrix  exponential  representation 

l£(^  =  1)  =  exp(7^),  (A.9) 

or,  with 

(  =  (A.IO) 

the  basis  transformation  in  terms  of  ^  is 

lg(0  =  exp  (^»)  .  (A.ll) 

Equation  (A.ll)  is  a  three-dimensional  parameterization  in  which  (  is  often  called  the 
orientation  vector.  This  representation  has  been  used  extensively  by  Faugeras  [40], 
for  example.  The  matrix  cross  product  operator,  (•)*,  yields  a  3  x  3  skew  symmetric 
matrix.  As  suggested  by  (A.9),  every  3x3  rotation  matrix  can  be  expressed  as  an 
exponential  function  of  some  3x3  skew  symmetric  matrix,  which  has  led  to  elegant 
studies  in  terms  of  Lie  algebra  as  an  approach  to  resolving  the  problem  of  rotation 
and  orientation  representations,  [68]. 

By  applying  the  well-known  Cayley-Hamilton  theorem  to  the  characteristic  equa¬ 
tion  of  the  skew  symmetric  matrix  and  using  the  fact  that  (  is  a  unit  vector,  the 
following  important  identity  is  obtained: 

({')'  + <’=0.  (A.12) 

A  result  of  (A.12)  is  that  the  highest  order  of  any  power  series  of  ^  is  two.  Conse¬ 
quently,  (A.9)  can  be  written  as 

12(7.0  =  i:4(7{'y 

i=oJ' 

=  h  +  sin(7)^  -I-  [1  -  cos(7)]  .  (A.13) 

With  the  identity 

(i?y=ie-h,  (a.14) 

the  result  in  Equation  (A.13)  can  also  be  written  as 

li(7i^  =  co8(7)l3  +  sin(7)^  +  [1  -  co8(7))^^.  (A.  15) 

The  angle  of  rotation,  7,  can  be  obtdned  from  either  by  using  Eigen  analysis 
with  Property  2  of  Section  A.l  and  recalling  that  the  trace  of  a  matrix  equals  the  sum 
of  its  eigenvalues,  or  by  noting  that  in  (A.13)  ^  is  skew  symmetric  and*  Tr{(^)*}  = 
—2.  In  either  case, 

'“(7)  =  i  {Tr(l|)  -  1) .  (A.16) 

’Th«  notation  TV{A}  denotes  the  trace  of  the  matrix  A. 
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In  addition,  the  axis  (  may  be  obtained  from  (A. 13)  with 

Because  a  rotation  about  ^  through  an  angle  7  is  equivalent  to  a  rotation  about 
through  an  angle  —7,  the  angle  of  rotation  may  be  restricted  to  the  interval  0  <  7  <  x 
for  unique  inversion  of  the  cosine  in  (A. 16).  In  this  case,  ^  computed  from  (A. 17)  to¬ 
gether  with  7  define  the  transformation  Ig.  However,  numerical  difficulties  may  arise 
when  is  symmetric  (7  =  0,  x)  for  which  the  expression  in  (A. 17)  is  of  indetermi¬ 
nate  form  (0/0)  indicating  the  mathematical  singularities  of  this  parameterization; 
when  7  =  0  the  axis  ^  is  undefined  and  when  7  =  x  the  sign  of  (  is  arbitrary. 

It  is  important  to  note  the  difference  between  the  axis,  of  the  basis  transforma¬ 
tion  operator,  I^,  and  what  is  commonly  called  the  “axis  of  rotation"  or  instantaneous 

angular  velocity  of  the  object  w(t)  =  (w£j£;(f ).  In  the  following  development,  all  time 
derivatives  are  with  respect  to  an  observer  in  the  earth-fixed  fraone,  and,  although 
the  subscript  ‘£’  is  omitted,  ^  is  expressed  in  Fe.  SeveraJ  identities  are  required  in 
the  derivation  of  w,  and  are  adso  used  to  write  7  and  ^  in  terms  of  w; 

1.  Because  ^  is  by  definition  a  unit  vector, 

=  1,  and 

•JT. 

( {  =  f(  =  o. 

which  means  that  (  is  always  orthogonal  to  its  time  derivative. 

2.  For  any  vector  a,  ^  x  x  a)  is  collinear  with  ^  and  hence 

^  X  X  (e  X  a))  =  0. 

In  terms  of  matrix  cross  product  operators,  (A.20)  is  written  as 

= 0. 

3.  The  set  {(, forms  a  right-handed  orthogonal  triad  and  hence 

=  0,  and  (A.22) 

m"  =  -I  (A.23) 

Similar  identities  can  be  applied  to  various  combinations  of  the  three  elements 
of  this  triad. 

4.  Two  standard  results  from  vector  analysis  give,  for  any  a,b,c  €51?, 

a  X  (b  X  c)  =  a^cb  -  a^bc,  and 

(a  X  b)  X  c  =  (ba^  -  ab^)c.  (A.24) 


(A.18) 

(A.19) 

(A.20) 

(A.21) 
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Substitution  of  (A. 13)  into  (A. 7)  followed  by  repeated  applications  of  identities 
(A. 12)  and  (A. 21)  results  in 

w*  =  7^  +  sin  7^  +  (1  -  cos 7)  •  (A.25) 

All  terms  in  (A.25)  au’e  cross  product  operators.  With  identities  (A.24)  and  using  the 
obvious  result  that  ==  following  result  holds  for  any  vector  c  €  IR^ 

=  ix(ixc)-ix((xc) 

=  (iT  -  if  )c  (A.26) 

=  (^x()xc. 

which  implies  that 

(A.27) 

This  result  allows  the  cross  product  operators  of  (A.25)  to  be  removed  which  gives, 

w  =  7^  +  sin 7I  +  (1  -  cos 7)^1.  (A.28) 


Equation  (A.28)  demonstrates  that  the  angular  velocity  has  com]>onents  on  three 
•  • 

orthogonal  axis  and 

Multiplying  both  sides  of  (A.28)  on  the  left  by  ^  and  using  identities  (A.22)  and 
its  counterparts  immediately  gives 


7  =  ^w. 


(A.29) 


Multiplying  both  sides  of  (A.28)  on  the  left  by  ^  and  using  identity  (A.23)  gives 


=  -  (1  -  co8(7))|  +  sin(7)^^,  (A.30) 


Multiplying  both  sides  of  (A.30)  on  the  left  by  ^  and  again  using  identity  (A.23) 
gives 

=  -8in(7)|-  (1  -  coa{ni))i'i.  (A.31) 

Equations  (A.30)  and  (A.31)  can  be  treated  as  two  equations  in  the  two  unknowns  { 
and  Solving  (A.30)  for  the  quantity  and  substituting  the  result  into  (A.30), 
followed  by  algebruc  manipulation  and  the  use  of  standard  trigonometric  identities 
gives 

^ (2)  *• 

Ex)uations  (A.29)  and  (A. 32)  together  define  the  relationship  between  the  time  deriva¬ 
tive  of  the  orientation  vector  and  angular  velocity. 


w. 


(A. 33) 
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The  complexity  of  this  expression  clearly  indicates  the  difficulty  of  implementing  an 
extended  Kalman  filter  with  the  angle-axis  parameterization  if  temporal  behavior  is 
modelled  directly  in  terms  of  angular  velocity. 


A. 3  Quaternion  Parameterization 

In  order  to  extend  the  operations  of  three-dimensional  vector  algebra  to  include 
multiplication  and  division,  Hamilton  (1843)  introduced  an  algebra  for  four-dimensional 
numbers,  or  quaternions.  The  objective  of  this  section  is  to  provide  an  introduction 
to  quaternion  calculus  with  emphasis  on  their  use  in  representing  change-of-basis 
transformations.  Results  of  the  previous  section  on  the  Euler  angle-axis  parameteri¬ 
zation  will  play  a  major  role  in  the  following  derivations.  Branets  and  Shmyglevski 
[83]  present  a  very  detailed  treatment  of  the  application  of  quaternions  to  rigid  body 
rotation  problems  and  provide  an  elegant  interpretation  of  rotation  in  terms  of  spher¬ 
ical  geometry.  Hughes  [84]  treats  several  parameterizations,  including  quaternions,  in 
applications  to  attitude  dynamics. 

A  quaternion  [29]-[37]  [65]-[70],  [83],  [84],  is  a  hypercomplex  number  which  is 
expressed  in  terms  of  basis  elements  consisting  of  the  real  number  -fl,  amd  three 
imaginary  units  i,  j,  k,  which  satisfy 

ij  =  -ji  =  k, 
jk  =  —kj  =  i,  and 
ki  =  —ik  =  j. 

A  quaternion,  q  can  be  written^  as 


=  91*  +  92}  +  93*  +  94-  (A.35) 


Quaternions  can  be  viewed  as  containing  the  real  numbers  [0, 0, 0,  a]^  with  the  real 
unit  1,  the  complex  numbers  [6,0,0,a]^  with  the  two  units  1  and  t,  and  the  vectors 
[6,  c,  d,  0]^  in  three-dimensional  space.  Some  additional  notation  is  required  for  de¬ 
velopments  of  this  section.  Both  quaternions  and  vectors  are  represented  by  bold 
face  lower-case  letters.  As  before,  matrices  are  represented  by  bold  upper-case  letters 
(with  the  exception  of  matrix  cross  product  operators).  The  vector  part  of  a  quater¬ 
nion  q  is  written  as  vect(q)  =  [91,92,93]^  while  the  real  or  scalar  part  of  q  is  denoted 

*Some  authors  place  the  scalar  quaternion  element,  94,  in  the  first  position  while  others  place  the 
scalar  element  in  the  fourth  position,  as  is  the  case  here,  when  representing  the  quaternion  in  vector 
form. 


9i 


L  94  J 


(A.34) 
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by  scal(q)  =  94.  With  this  notation, 

q  =  vect(q)  +  8cal(q).  (A.36) 

A  quaternion  formed  from  a  vector  r  6  IR>^by  adding  a  zero  scalar  part  will  be  denoted 
by  quat(r). 

Addition  of  quaternions  aoid  their  multiplication  by  a  scalar  (which  can  also  be 
viewed  as  a  quaternion)  are  performed  in  the  same  way  as  in  an  ordinary  vector  space. 
Quaternion  multiplication,  which  is  denoted  here  with  the  symbol  ‘o*,  is  defined  in 
terms  of  the  products  of  basis  elements  shown  in  (A.34).  Quaternion  multiplication 
is  associative,  and  distributive  with  respect  to  addition,  but  it  is  noncommutative 
which  is  evident  from  the  last  three  rows  of  (A.34),  i.e.  ij  /  ji  for  example.  For  any 
quaternion  q,  the  conjugate,  q*,  of  q  is  defined  as 

q*  =  -  vect(q)  +  scal(q),  (A.37) 

and  the  norm  is  defined  as  the  Euclidean  length  of  the  4-vector  representing  q: 

llqll  =  y/ql  +  ql  +  ql  +  (A.38) 

=  v/qoq*,  (A.39) 

where  the  second  equation  above  follows  from  the  rules  of  multiplication  given  in 
(A.34).  Provided  ||q||  ^  0,  the  inverse  of  q  is 


-1  A  q 

=nF' 


(A.40) 


and  satisfies  q  o  q~*  =  q”*  o  q  =  1.  For  any  quaternion  q,  the  corresponding  unit 
quaternion  q  is  given  by 

^  "  H’ 

and  note  that  q~*  =  q*.  The  set  of  unit  quaternions  can  be  placed  in  a  two-to-one 
correspondence  with  elements  of  the  three  dimensional  rotation  matrices. 

In  Equation  (A.  13),  the  transformation  from  Fq  to  Fe  was  written  as 


Ig  =  fc  +  .il.(7)C  + 11  -  C<»(7)1  ({■)’ . 


(A.42) 


Using  the  familiar  double-angle  formulas 


sin(7) 

=  2co8(|)sin(|),  and 

(A.43) 

C08(7) 

(A.44) 

=  l-2sin’(|) 

(A.45) 
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in  (A.42)  gives 

lS  =  l3-2co»(|)[-sin(2)4]'  +  2^[|-Bm(2)(]'j  .  (A.46) 

The  set  of  four  parameters  where 

fj  =  -8in(^)i 

e  =  co8(|)  (A.47) 

are  called  Evltr  Parameters  [84]  and  can  be  identified  with  the  unit  quaternion 

q  =  [»?^£r-  (A.48) 

With 

q  =  llq||(»i  +  £)>  (a.49) 

substitution  of  (A.47)  into  (A.46)  gives  the  following  matrix  representation  for  in 
terms  of  the  elements  of  a  quaternion  q: 

91-92-93  +  94  2(9193  +  9394)  2(9193-9294) 

2(9192  -  9394)  -9i  +  92  -  93  +  94  2(9293  +  9194) 

2(91 93  +  9394)  2(9293  -  9194)  -9i  -  92  +  93  +  94  . 

(A.50) 

It  can  be  verified  by  direct  computation,  using  the  multiplication  rules  in  (A. 34), 
that  for  any  vector  r,  the  representation  of  r  in  Fe  can  be  obtained  from  its  repre¬ 
sentation  in  Fo  according  to 

te  =  l£(q)ro 

=  vect  o  quat(ro)  o  q| 

=  vect  {q*  o  quat(ro)  0  q)  (A.51) 


This  property  of  quaternion  calculus  is  extremely  useful  when  multiple  consecutive 
transformations  (or  rotations)  must  be  performed  as  in  the  roll-pitch-yaw  formulation 
which  will  be  demonstrated  in  Section  A  4. 

Temporal  behavior  of  the  unit  quaternion  q(f)  can  be  obtuned  through  differen¬ 
tiation  of  9  and  £  in  (A.47)  and  the  use  of  results  in  Equations  (A. 29)  and  (A. 32)  of 
Section  3.5.1.  With  some  algebraic  manipulation,  one  easily  obtains 


V 


=  -^(9‘  +  el3)  w,  and 

1  j 
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where  w  =  =  [u>i,uj2,ti;3]^  is,  as  before,  the  angular  velocity  vector  of  Fo  with 

respect  to  Fe  expressed  in  Fe-  Two  equivalent  expressions  for  q(t)  are  then  given  by 
using  (A. 48)  in  (A. 52)  and  (A. 53): 


where 

^  = 
dw 

n[w]  = 


-  A 

q  =  n[w(<)]q(<), 


94 

-93 

92 

1 

93 

94 

~91 

,  and 

-93 

9i 

94 

.  9i 

92 

93  . 

■ 

0 

-W3 

tl>2 

-Wi  ■ 

1 

t03 

0 

-Wl 

—  W2 

2 

—  tWj 

lUi 

0 

—W3 

Wl 

W2 

ti>3 

0 

(A.54) 

(A.55) 


(A.56) 


(A.57) 


Both  (A.56)  and  (A.57)  aje  required  in  the  extended  Kalman  filtering  equations. 

With  constant  angular  velocity,  the  solution  of  (A.55)  with  initial  conditions  given 
as  q(<o)  is  simply 

q(<)  =  exp  [ft  •  (<  -  to)]  q{to)-  (A.58) 

Note  that  ft  is  skew  symmetric  and,  since  the  matrix  exponential  of  a  skew  symmetric 
is  always  orthogonal,  time  propagation  of  the  quaternion  in  (A.58)  with  unit  length 
is  maintjuned.  It  can  be  verified  by  direct  computation  that 


n’  =  -j||w||’I,,  (A.59) 

which  implies  that  the  highest  power  of  ft  in  a  power  series  is  one.  Expansion  of 
(A.58)  followed  by  repeated  application  of  (A.59)  gives 


q(0  =  cos  (||w|l(/  -  to)/2)  I4  +  jj^  sin  (llw||(t  -  <o)/2)  ft 


q(<o)-  (A.60) 


Ekiuation  (A.60)  is  only  valid  when  w  is  constant.  Closed  form  solutions  for  q(t)  are 
also  available  [34]  for  rotation  with  constant  precession. 


A.4  Roll-Pitch- Yaw  Parameterization 

The  roll-pitch-yaw  representation  [38, 63,  82]  is  a  particular  case  of  an  Euler  angle 
parameterization  of  the  three-dimensional  rotation  group.  The  roll,  pitch,  and  yaw 
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angles,  denoted  9,  and  0,  respectively,  define  an  ordered  sequence  of  three  plane 
rotations  which  can  be  used  to  express  the  basis  transformation  1%  as 

l£  =  exp(V>e^)exp(tfei)exp((^*,),  (A.61) 

where  e,,  i  =  1,2,3,  are  the  standard  basis  vectors  oflR^.  Expanding  the  exponentials 
with  (A. 13)  gives  the  three  plane  rotations, 

r  1  0  O' 

exp(^*)  =  0  coe(^)  —  sin(^)  , 

0  sin(^)  cos(^) 

cos(d)  0  sin(0) 

exp(de*)  =  0  10,  and  (A. 62) 

—  sin(^)  0  cos(0) 

cos(0)  —  sin(V’)  0 

exp(V’e3)  =  sin(^)  co8(\/>)  0  . 

[0  0  1  , 

The  angles  <f>,  6,  and  tp  have  positive  sense  defined  by  the  right  hcind  rule  about  their 
respective  rotation  axis.  Specifically,  the  rotation  from  Fg  to  Fo  is  carried  out  by  the 
following  sequence: 

1.  rotate  about  kg  by  the  yaw  angle  fj?; 

2.  rotate  about  the  axis  [— sin(V’),  cos(\^),0]^  by  the  pitch  angle  9;  and 

3.  rotate  about  the  axis  (cos(V>)  cos(f?),  sin(V’)  cos(^),  —  sin(0)]^  by  the  roll  angle  4>- 

In  order  to  avoid  ambiguities  in  these  angles  their  ranges  are  often  limited  to 

-r  <  ^  <  X, 

-f  <  ^  <  i,  and  (A.63) 

-X  <  ^  <  X. 

The  angular  velocity  of  Fq  with  respect  to  Fe  expressed  in  Fe,  w  =  [w^Je,  is 
given  in  terms  of  the  time  derivatives  of  roll,  pitch,  and  yaw  angles  as 

k 

[w^Ie  =  iRPY  9  ,  (A.64) 

where 

'  cos(^)cos(9)  —  sin(0)  0 

J/tpy  =  sin(0)cos(0)  cos(0)  0  (A.65) 

—  sin(^)  0  1 
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contains  the  three  rotational  axis  (from  items  1,2,  and  3  above)  as  its  columns  (which 
follows  from  the  vector  addition  property  of  angular  velocity).  The  mathematical 
singularities,  6  =  ±x/2,  characterize  the  orientations  where  the  determinant  of  the 
Jacobian  matrix  det[JApy]  =  —  cos(^)  vanishes.  At  6  =  x/2,  the  roll  and  yaw  angles 
are  indistinguishable  in  the  sense  that  the  rotation  matrix  can  be  written  as 


0 

0 

1 


sin(^  —  0) 
C08(^  —  xj)) 
0 


cos(^-V') 
-sin(^-^)  , 

0 


(A.66) 


and  hence  depends  only  on  6  =  x/2  and  the  difference  ^  —  0. 

The  sequence  of  roll,  pitch  2aid  yaw  transformations  can  be  represented  in  terms 
of  corresponding  unit  quaterions. 


’  —  sin(^/2) 

0 

0 

_  A 

0 

_  A 

—  sin(d/2) 

_  A 

0 

= 

0 

cos(^/2) 

,  qfl  = 

0 

cos(d/2) 

,  q^  — 

—  sin(^/2) 
cos(61/2) 

(A.67) 


according  to 

te  —  l^ro 

=  exp(V’eJ)  [exp(^e|)  (exp(^l)ro)] 

=  vect  {a;  o  A3  0  Aj  0  quat(ro)  o  o  q*  o  q^| .  (A.68) 

A  single  equivalent  quaternion  representing  the  basis  transformation  can  be  then 
defined  as 

qequiv  =  0  q,  o  q^.  (A.69) 

Noting  that  multiplication  and  conjugation  rules  for  quaternions  imply  that 

A^y  =  A;oA5oA;  (A.70) 


the  transformation  operation  can  be  written  as 

I^ro  =  vect  {q^^  o  quat(ro)  o  q^quiv} .  (A.71) 

The  Euler  axis-angle  representation  corresponding  to  a  particular  set  of  roll,  pitch 
and  yaw  angles  can  then  easily  be  extracted  from  the  equivalent  quaternion. 

By  forming  the  product  of  the  three  plane  rotations  in  (A.61)  and  defining  the 
elements  of  as  iij,  the  following  algorithm  can  be  used  to  extract  a  valid  set  of  roll 
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pitch  (^),  and  yaw  {ip)  angles: 

Check  for  singulairity  with  e  1 
if((|^n|<e)fc(K«|<c)), 

<t>  =  0.0; 

Ip  =  -atan2(£i2,^22); 
if  (^31  ^  0.0), 

e  =  x/2; 

else 

e  =  -x/2;  (A.72) 

else 


<p  =  atan2(/32,^33); 


Ip  =  atan2(/2i,<n); 


end. 


Functions  atan2(y,z)  and  atan(z)  return,  respectively,  the  four-quadrant  arctangent 
of  y/x  and  the  two-quadrant  arctangent  of  z,  and  are  available  in  most  programming 
languages.  Note  that  in  the  singular  orientations  {0  =  ±t),  the  roll  angle  is  arbitrarily 
set  to  zero. 


DRES-SR-577 


UNCLASSIFIED 


A. 14 


DRES-SR-577 


Appendix  B 

Kalman  Filtering  Review 


This  appendix  provides  a  review  and  discussion  of  the  linear  discrete-time  Kalman 
filter,  the  discrete-time  extended  Kalman  filter,  and  the  continuous-discrete  Kalman 
filter.  Local  iterations  which  lead  to  the  iterated  extended  Kalman  filter  are  also 
introduced.  Applications  of  Kalman  filtering  to  trajectory  estimation  problems  has 
received  widespread  attention  since  the  introduction  of  Kalman's  sequential  state 
estimation  technique  [60].  Chang  and  Tabaczynski  [62]  give  a  thorough  discussion  of 
many  important  aspects  of  Kalman  filtering  approaches  for  target  tracking  problems. 
In  general,  the  trajectory  estimation  problem  is  one  of  nonlinear  estimation  for  which 
the  optimal  (conditional  mean)  nonlinear  estimator  cannot  be  realized  with  a  finite¬ 
dimensional  implementation.  Furthermore,  the  mathematical  model  representing  the 
target  trajectory  is  seldom  exact.  Consequently,  most  practical  implementations  of 
nonlinear  tracking  filters  are  suboptimal. 

The  Extended  Kalman  Filter  (EKF)  is  a  suboptimal  approach  which  applies  lin¬ 
ear  Kalman  filtering  theory  to  a  linear  approximation  of  the  nonlinear  problem.  A 
rigorous  development  of  filtering  equations  requires  the  use  of  stochastic  integral  and 
differential  equations  and  mathematical  expectation  conditioned  on  a-algebras  gener¬ 
ated  by  the  observation  process,  and  formulates  solutions  through  Ito  calculus  ([74, 81, 
for  example]).  In  applications,  however,  the  somewhat  more  intuitive  approaches  us¬ 
ing  "formzd”  manipulations  of  white  noise  processes  and  expectation  conditioned  on 
sets  of  observations  are  often  preferred  [62],  [65],  [23]-[44].  This  approach  will  also 
be  taken  in  the  following  review  of  the  EKF  formulation.  The  following  subsections 
draw  primarily  from  [74]-[78]. 

In  many  cases,  the  target  tracking  problem  can  be  formulated  in  terms  of  discrete¬ 
time  system  (plant)  and  measurement  models.  In  the  case  of  image-based  tracking, 
measurements  are  taken  from  image  frames  which  arrive  at  discrete  instants  in  time. 
The  measurement  model  for  this  class  of  problems,  therefore,  is  always  discrete.  In 
addition,  the  perspective  projection  image  formation  model  results  in  a  nonlinear 
measurement  model.  If  the  system  model  is  continuous  in  time,  it  can  often  be 
discretized,  or  the  “continuous-discrete"  [74]  form  of  the  filter  can  be  employed. 

Notation  and  filtering  equations  for  the  discrete-time  linear  state  estimation  prob- 
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lem  are  reviewed  in  Section  B.l,  with  extensions  to  approximate  filters  for  the  nonlin¬ 
ear  discrete-time  problem  in  Section  B.2  and  the  continuous-discrete  form  of  extended 
Kalman  filtering  in  Section  B.3.  Finally,  Section  B.4  describes  local  iterations  which 
enhance  filter  performance  in  the  presence  of  nonlinear  measurement  models. 

B.l  Discrete-Time  Kalman  Filter 

Consider  a  linear  discrete-time  dynamical  system  model  given  by 

x(jk  -b  1)  =  «(ifc)x(ik)  -f  B(jt)u(jb)  +  G(ik)w(jt),  (B.l) 

where  A;  ~  an  integer  valued  sample  index,  time  t  =  kT  where  T  is  the  sample  period, 
and 

xeiR"  ~  vector  of  state  variables, 
u  6 IR*  ~  deterministic  input  vector, 
w  €  IR’’  ~  zero  mean,  white,  Gaussian,  system  noise 
process  with  covariance  matrix  Q(k), 

$  €  IR"’'"  ~  state  transition  matrix,  (B-2) 

B  €  IR”^’  input  matrix  mapping  the  input  vector 
u  into  the  state  space,  and 
G  €  IR"’''’  matrix  mapping  the  noise  process 
w  into  the  state  space. 

The  linear,  discrete-time  measurement  model  is  given  by 

z(it)  =  H(jb)x(it)-l-v(jfc),  (B.3) 

where 

z  €  IR*”  ~  vector  of  observations  or  measurements, 

H  6 IR"’*”*  'N/  measurement  matrix  relating  state  variables 

to  measurement  variables,  and  (B.4) 

V  €  H*”  ~  zero  mean,  white,  Gaussian,  measurement  noise 
process  with  covariance  matrix  R(ib). 

With  definitions  (B.2)  and  (B.4),  the  system  and  measurement  noise  processes 
satisfy 

S  {w(*))  =  0  £  {w(*)w’'(7)}  =  (B.5) 

£  {v(t))  =  0  £  {v(k)y^(j)}  = 

where  £{•}  denotes  expectation  and  6jk  is  the  Kronecker  delta  product.  In  the  present 
analysis,  w  and  v  are  also  assumed  to  be  uncorrelated,  i.e., 

£{w(t)v^0')}=0,  (B.6) 
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for  a\l  integers  j  and  k. 

The  expectation  of  the  state  x  at  time  step  k  conditioned  on  all  measurements  up 
to  time  step  j  is  denoted  by 

x(t|j)i£r{x(*:)|Z,)  (B.7) 

where  Zj  =  {z(«)>  *  =  represents  the  set  of  all  measurements  up  to  amd 

including  the  jth  time  step.  It  is  precisely  the  conditional  mean  in  (B.7),  usually  with 
j  =  jfc,  which  the  optimal  filter  is  required  to  estimate.  In  the  case  of  linear  system  and 
measurement  models,  assuming  exact  mathematical  models  and  characterization  of 
the  noise  processes,  the  Kalman  filter  provides  a  recursive  “formula”  for  this  optimal 
estimate.  It  is  well  known  that  in  this  case  the  Kalman  filter  provides  an  unbiased, 
minimum  variance,  consistent  estimate  x(ib|Jb),  which  is  also  the  maximum  likelihood 
estimate  of  the  true  state  x{k). 

The  error  between  the  filter  state  and  the  true  state  is  given  by 

x(k\j)  =  x{k)  -  x(fc|i),  (B.8) 

with  covariance  matrix 

P(iL;)^£:{x(i|;)x7'(ltl»}.  (B.9) 

The  development  of  the  filtering  equations  assumes  that  the  true  initial  state  of 
the  system  x(0)  N(xo,Po)^*  Throughout  this  work,  it  is  assumed  that  an  initial 
estimate,  x(0|  —  1),  of  the  mean  of  the  true  initial  state,  5{x(0)},  is  available  just  prior 
to  taking  initial  measurements,  z(0).  In  theory,  then,  the  recursive  filter  is  initialized 
with 

x(0|  -1)  =  S  {x(0)}  =  xo,  and 

P(0|  -  1)  =  f  {x(0|  -  l)x7'(0|  -  1)}  ^  P„.  (B.10) 

In  practice,  however,  the  filter  is  often  initialized  with  an  unbiased  estimator  of  5co 
for  which  some  a  priori  statistical  information  is  known  of  the  form 

x(0i-l)~N(xo,Po).  (B.ll) 

After  Jb  —  1  measurements  have  been  taken,  the  recursive  equations  of  the  linear 
Kalman  filter  are  summarized  as  follows: 

1 .  State  and  covariance  prediction  (or  time  update)  - 

x{k\k-\)  =  «(ifc-l)x(ib-l|ib-l)  +  B(jb-l)u(ib-l),  and 
P(it|Jfc-l)  =  ♦(i-l)P(jk-l|jfc-l)*^(jt-l) 

+  G(ik-l)Q(ib-l)G^(ib-l).  (B.12) 

‘The  notation  N(in,C)  denotes  a  normally  distributed  random  variable  with  mean  m  and  co- 
variance  C. 
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2.  Measurement  update  computed  following  the  Jbth  measurement  - 
K{k)  =  P(ib|jb  -  l)H^(ib) 

•  [H(it)P(ik|ifc  -  l)H^(jfc)  +  R{jb)]‘* ,  {B.13) 

x{k\k)  =  x(ifc|ik  -  1)  +  K(Jt)  {z(ib)  -  H{ib)x(it|Jt  -  1)] ,  and 
P{k\k)  =  (I-K(ifc)H(Jb)lP(jb|ib-l). 

An  equivalent  form,  known  as  the  ‘^information  form”,  of  the  measurement  update 
stage  of  the  filter,  which  is  often  used  in  place  of  (B.13),  is  given  by 

P-‘(jb|jb)  =  P-^k\k-l)  +  BT{k)R-'{k)H{k), 

K{k)  =  P(lfc|ib)H^(ib)R-‘(ib),  and  (B.14) 

x(ib|Jb)  =  xik\k-l)  +  K{k)[z{k)-Hik)x{k\k-\)]. 

In  any  implementation  of  the  above  filtering  equations,  the  following  parameters 
must  be  specified  in  addition  to  the  measurement  sequence:  1)  an  initial  state  estimate 
x(0|  —  1);  2)  an  estimate  of  the  initial  error  covariance  Po;  3)  the  system  or  plant  noise 
covariances  Q(il:);  and  4)  the  measurement  or  observation  noise  covariances  R(il:). 
The  initial  state  estimate  and  its  error  covariance  matrix  often  result  from  a  batch 
estimation  technique  applied  to  the  first  few  image  frames  [30,  32, 38].  In  simulations, 
the  measurement  noise  covariance  is  known,  however,  in  applications  to  real  imagery, 
determination  of  the  actual  measurement  noise  is  difficult  since  this  involves  modeling 
of  various  sources  of  error  in  the  imaging  system  and  feature  detection  algorithms. 
Specification  of  the  system  noise  covariance  Q(^)  is  seldom  exact  in  object  tracking 
problems  since  unmodeled  dynamics  resulting  from  manoeuvring  object  trajectories 
are  difficult  to  quantify.  As  a  result,  Q(A:)  together  with  the  matrix  G{k)  are  often 
treated  as  “tuning”  parameters  which  are  selected  to  pve  satisfactory  convergent 
behavior  of  the  filter  over  a  wide  range  of  applications. 

In  Equation  (B.12)  the  previous  state  estimate  x(il;—  l|ik— 1)  is  propagated  in  time 
between  measurements  according  to  the  noise-free  dynamic  system  model  to  obtsun 
a  state  estimate  x(ik|jb  —  1)  just  prior  to  a  measurement  event.  Once  measurements 
are  available,  the  state  estimate  is  corrected  in  (B.13)  by  an  amount  proportional  to 
the  current  error  [z(jb)  -  H()b)x(ib|Jk  —  1)]  called  the  innovation.  The  proportionality 
factor  K(Jk)  is  called  the  Kalman  giun  which,  from  (B.14),  is  proportional  to  the  un- 
certdnty  in  the  state  estimate  P(A;|il;)  and  inversely  proportional  to  the  measurement 
uncertdnty  R(il;).  The  system  noise  covariance  Q(ik)  which  is  mapped  to  the  state 
space  through  G(ib)  adds  a  positive  definite  matrix  to  the  filter  error  covariance  ma¬ 
trix  during  each  time  update  (B.12)  which  reflects  a  “diffusion”  of  confidence  in  the 
state  estimate  over  the  time  interval  between  measurements.  In  the  absence  of  system 
noise  (Q  =  0)  the  error  covariance  and  gain  matrices  may  decreases  monotonically  in 
the  presence  of  measurement  data,  depending  on  the  state  transition  matrix  through 
the  similarity  transformation  in  (B.12),  i.e.,  consider  (B.14)  for  the  scalar  case  with 
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H  =  1.  In  this  case,  the  filter  eventually  neglects  further  measurements  and  operates 
solely  on  the  system  model  which,  with  Q  =  0,  is  assumed  to  be  exact. 


B.2  Discrete-Time  Extended  Kalman  Filter 


The  problem  of  estimating  the  state  of  a  nonlinear  system  can  be  formulated  as  an 
extension  of  mean  square  estimation  to  dynamic  systems.  Formally,  the  method  for 
deriving  the  fundamental  equations  of  the  optimal  filter  is  quite  similar  to  that  used  to 
design  the  optimal  linear  filter  (Kalman  filter)  for  linear  systems  with  Gaussian  noise 
processes.  However,  in  the  nonlinear  case,  the  explicit  analytical  solution  cannot,  in 
general,  be  found  and  hence  approximate  numerical  solutions  are  often  considered. 

In  the  case  of  linear  systems,  all  of  the  variables  and  processes  involved  (states, 
noises,  measurements,  estimates,  and  errors)  are  usually  assumed  to  be  normally 
distributed.  Therefore,  mean  values  and  covariance  matrices  are  ‘‘sufficient  statistics.” 
In  the  nonlinear  case,  on  the  other  hand,  the  state,  observation,  and  estimation 
processes  are  not,  in  general,  normally  distributed  even  if  the  input  noise  sources 
are.  As  a  result,  the  entire  probability  distribution  may  be  required  for  optimal  filter 
design.  In  addition,  the  filter  performance  and  parameters  are  often  dependent  on  the 
filter  estimate  so  that  it  is  even  more  difficult  to  obtain  bounds  for  filter  performance. 

Consider  the  nonlinear  system  model  ^ven  by 

x(k  +  1)  =  f[x(fc)]  +  B(fc)u(fc)  +  g[x(l:)]w(t),  (B.15) 

and  the  nonlinear  measurement  model 


z{k)  =  h(x(fc)]  +  v(l:). 


(B.16) 


where  f  :  if*  •-»  if*,  g  :  if*  (if**>*),  and  h  :  if*  if"  are,  in  general,  nonlinear 
functions  of  the  state  vector  x{k).  The  mapping  of  the  input  vector  u  to  the  state 
space  may  also  be  nonlinear,  but  this  case  will  not  be  considered  here  and,  in  any 
event,  does  not  alter  the  filter  structure  significantly. 

The  Extended  Kalman  Filter  (EKF)  is  based  on  the  concept  that  if  the  nonlinear¬ 
ities  in  f  and  h  are  sufficiently  smooth,  these  functions  can  be  expanded  in  a  Taylor 
series  and  approximated  by  linear  terms.  Furthermore,  if  the  filter  works  properly,  it 
will  provide  a  reasonable  estimate  of  the  conditional  mean  £^{x(Jb)|Zk}  which  is  what 
the  optimal  filter  is  required  to  estimate.  For  this  reason,  the  Taylor  series  expansion 
of  nonlinear  terms  is  formulated  about  the  most  recent  state  estimate.  The  primary 
consequence  of  this  “feedback”  of  the  estimate  into  the  model  is  that  the  EKF  is  a 
nonlinear  estimation  technique. 

With  the  definitions 


*{k) 


dx 


> 

X=*(k|k) 


(B.17) 
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G{k) 

H(fc) 


A 

A 


g[x(ilfc)l, 

ah[xi| 


and 


dx 


x=t(k\k-\) 


(B.18) 

(B.19) 


and  neglecting  all  terms  beyond  first  order  in  the  Taylor  series  expansions,  approxi¬ 
mate  linear  system  and  measurement  models  can  be  written,  respectively,  as 


where 


x(fc-|-  1) 

=  ♦(fc)x(ib)  -1-  ix{k)  -f  G(jt)w(jb),  and 

(B.20) 

z(fc) 

=  H{k)xik)  +  yik)  +  y{k), 

(B.21) 

ll> 

f (x(ik|Jb)]  -  « (ib)x(ib|Jfc)  -1-  B(ik)u(ib),  and 

(B.22) 

y{k)  = 

h[xik\k-l)]-H{k)x{k\k-l). 

(B.23) 

The  EKF  equations  are  formulated,  essentially,  as  the  standard  Kalman  filtering 
equations  for  the  linear  system  given  by  (B.17)-(B.23). 

After  k—l  measurements  have  been  taken,  the  recursive  equations  of  the  extended 
Kalman  filter  are  summarized  as  follows: 


1.  State  and  covariance  prediction  (or  time  update)  - 

x(Jblfc-l)  =  f[x(ib-l|ik-l)l-|-B(jk)u(ib),  and  (B.24) 
P{k\k-1)  =  «(jb-l)P(jfc-l|jb-l)«^(jk-l) 
-|-G(ifc-l)Q{ib-l)G^(ib-l). 

2.  Measurement  update  computed  following  the  ikth  measurement  - 

K(Jt)  =  P(it|ib-l)H^(it) 

.  [H(jfc)P(ik|ib  -  l)H^(jb)  +  R(jk)]‘' ,  (B.25) 

x{k\k)  =  x(Jfc|ifc-l)-|-K(ifc){z(ib)-h[x(jb|jt-l)]},  and 

P{k\k)  =  \l-K{k)ll{k)]P{k\k~l). 

The  accent  on  P  has  been  used  to  emphasize  the  fact  that  the  covariance  estimate  is 
dependent  on  the  filter  estimate. 


B.3  Continuous-Dbcrete  Extended 
Kalman  Filter 

In  Section  B.2,  both  the  system  and  measurement  models  were  assumed  to  be 
available  in  discrete-time  form.  In  general,  however,  the  system  model  is  often  devel¬ 
oped  in  continuous-time  form,  while  measurements  are  taken  at  discrete  instants  in 
time. 
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Consider  a  continuous-time  system  which  is  described  by  the  stochastic  differential 
equation 

x(0  =  f(x,t)  +  G(0w(t),  t>0,  x(0)~  yV(xo,Po),  (B.26) 

where  w(t)  is  a  zero-mean  white  Gaussian  noise  process  with 

e  {w(<)wV)}  =  -  ’■).  (B.27) 

and  6{t  —  t)  is  the  Dirac-delta  function.  The  system  model  may  also  depend  on  a  de¬ 
terministic  input  vector  u(t).  To  simplify  the  notation,  this  dependence  is  not  shown 
explicitly  here  and,  in  any  event,  does  not  alter  the  filter  structure  significantly.  As 
in  the  discrete-time  case  of  Section  B.2,  the  nonlinear  system  model  of  (B.26)  is  lin¬ 
earized  approximately  through  a  first-order  Taylor  series  expansion  about  a  nominal 
trajectory  x(t)  which  satisfies 

x(0  =  f(x,t),  t  >  0.  (B.28) 

The  state  perturbation  is  defined  as 

x(0  =  x(<)  -  x(<),  (B.29) 

which  satisfies  the  stochastic  differential  equation 


=  f(x,t)-f(x,<)-|-G(f)w(f) 

«  F[<;x]x(0  +  G(f)w(0,  (B.30) 

with  the  initial  condition  x(0)  ~  ^(xo  -  x(0),Po)  and  where 

F(f;x]i 

ox 

It  should  be  noted  that  F  is  evaluated  at  particular  values  of  x(<)  and  hence  is  a 
function  of  t  only.  Specifically,  as  in  the  discrete-time  case  of  Section  B.2,  the  nominal 
trajectory  is  taken  as  the  most  recent  state  estimate  which,  for  the  linearized  system 
model,  is  given  by 

x(tk)  =  xik\k).  (B.32) 

In  principle,  equation  (B.30)  can  be  written  in  discrete-time  form  as 

x(*:  +  1)  =  4(<*+i,tk)x(4)-|- Wi(fc-|- 1),  (B.33) 

where  is  the  state  transition  matrix  which  satisfies  [74] 


d^(<,T) 

dt 

4(t,t)  = 
4(f,r)4(r,C)  = 


F[f;x(0]4(f,T), 
In,  Vr, 

♦(«,C),  Vf,r,C, 


(B.34) 
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and  Wj(A:  + 1)  is  a  zero-mean,  white  Gaussian  sequence,  wj(fc-|- 1)  N{0,Q,{k  +  l)), 

where 

Qi(t  +  1)=  /'”‘*(i*+„r)G(r)Q(r)G’'(T)*’'(<*+i,>-)dr.  (B.35) 

If  F  is  approximately  constant  over  the  interval  where  tk+\  —ik  =  Tt,  the 

state  transition  matrix  for  each  sample  interval  can  be  estimated  as 

4(<*+i,<*)  =  exp(Fr*).  (B.36) 

In  general,  however,  the  computation  of  the  state  transition  matrix  is  often  difficult 
and,  therefore,  replaced  by  numerical  integration  of  the  Riccati  covsiriance  equation 
[741 

=  F[(;x(<|(»)lP((|l») 

+  ii(<l<k)]  +  G(0Q(i)G’-(i).  (B.37) 

The  nonlinear,  discrete-time  measurement  model  is  agiun  given  by  (B.16)  and  is 
treated  in  a  manner  identical  to  that  used  in  Section  B.2.  The  extended  Kalman  filter 
is  then  formulated  based  on  the  approximate  linear  perturbation  equation  (B.30),  or 
its  discrete  form  (B.33),  and  the  linear  approximation  of  the  measurement  model 
(B.21).  However,  in  this  case,  the  state  prediction  is  computed  by  integration  of  the 
nonlinear  noise-free  system  differential  equation.  After  ib  —  1  measurements  have  been 
taken,  the  recursive  equations  of  the  continuous-discrete  extended  Kalman  filter  are 
summarized  as  follows: 

1.  State  and  covariance  prediction  (or  time  update)  -  Either 

x(ib|fc  -  1)  =  x{k  -  l|ik  -  1)  -1-  f(x(fl<k_i),t]d<  and 

^(t|t-l)  =  P(l-llt-l)+ /“  {B.38) 

A 

where  P(<;x(<|ffc_i)]  is  defined  in  (B.37),  or  equivalently, 

x(jk|ib-l)  =  4(jb  -  l)x(ifc  -  l|ib  -  1)  (B.39) 

P(jb|*-1)  =  4(ib-l)P(fc-l|it-l)4^(fc-l)-fQ,(ib). 

2.  Measurement  update  computed  following  the  Jbth  measurement  - 

K(lb)  =  P(jb|jb  -  l)H^(ib) 

•  [H(ib)P(ib|ib  -  l)H^(it)  +  R{k)]~' ,  (B.40) 

x{k\k)  =  x{k\k  -  1)  +  K{k)  {z{k)  -  h[x(iblfc  -  1)]}  ,  and 
P(jfe|jt)  =  [I-K(ib)H(ib)]P(jfc|ifc-l). 

Again,  the  accent  on  P  has  been  used  to  emphasize  the  fact  that  the  covariance 
estimate  is  dependent  on  the  filter  estimate. 
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B.4  Local  Iterations 

The  Iterated  Extended  Kalman  Filter  (lEKF)  is  designed  to  reduce  the  effect 
of  measurement  nonlinearities  on  the  performance  of  the  linear  filter  applied  to  a 
linecirized  nonlinear  system  [74,  p.  279].  Between  observations  the  conditional  mean 
and  covariance  matrix  propagate  according  to  the  equations  of  (B.24).  Following  the 
time  update  of  the  state  and  covariance  from  time  step  it  —  1  to  time  step  k,  and  with 
the  A;th  measurement  available,  the  iteration  is  initialized  with 

x{k\k)Q  =  x{k\k  -  1).  (B-41) 

Iterations  are  carried  out  on  the  measurement  update  equations  in  (B.25)  according 
to  the  following  algorithm: 

1.  Linearize  the  measurement  equations  about  the  current  iterate  - 

H(lt).  =  ^  ,  (B,42) 

2.  Compute  the  Kalman  gain  - 

K(t).  =  P(fclli  -  l)H’’(k), 

•  [H(t)„P(t|t  -  l)H’’(t)„  +  R(t)]'’ ,  (B.43) 

3.  Compute  the  measurement  update  - 

X(‘I*)«1  =  ic(t|t  -  1)  +  K(t),  {*(*)- h(x(*|k),l 

-  H(t),  |x(lt|k  -  1)  -  x(k|t).))  .  (B.44) 

Iteration  is  terminated  when  there  is  no  significant  change  in  consecutive  iterates. 
Then  the  last  iterate,  x{k\k)N  say,  is  taken  for  the  estimate  x(it|jb)  and  the  covariance 
matrix  is  computed  based  on  the  linearization  about  x{k\k)N, 

P(A:|*)  =  [I  -  K{k)NH{k)s]  P{k\k  -  1).  (B.45) 

The  iterative  procedure  described  by  (B.42)-(B.44)  can  be  inte'^preted  as  a  modi¬ 
fied  Newton-Raphson  search  for  the  conditional  mode  of  the  posterior  density  of  the 
state  estimate  given  the  measurement  at  time  step  k,  assuming  the  prior  density  is 
Gaussian  [74,  pp.  349-351].  The  mode  of  the  posterior  density  is  then  used  for  the 
conditional  mean.  In  nonlinear  problems,  these  local  iterations  can  be  expected  to 
produce  a  biased  estimate  since  the  mode  is  not,  in  genera],  equivalent  to  the  mean. 
However,  as  the  error  variance  becomes  small,  so  does  the  bias  in  this  estimate. 
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Appendix  C 

Cramer-Rao  Covariance  Bounds 


The  extended  Kalman  filter  is  generally  a  suboptimal  state  estimation  technique. 
A  natural  consideration  in  any  estimation  problem  is  how  well  the  proposed  technique 
performs  when  compared  to  the  optimal  performance  possible.  A  powerful  result 
which  provides  a  performance  assessment  of  parameter  estimation  techniques  is  the 
Cramer-Rao  inequality  [79].  The  following  derivation  draws  from  methods  outlined 
in  [61,  62,  74,  79,  80]. 

Consider  a  nonlinear  discrete-time  estimation  problem  with  noise-free  system 
model 

x(*: -f  1)  =  f[x(fc)]  (C.l) 

and  measurement  model 

z(fc)  =  h[x(fc)]  +  v(fc),  (C.2) 

where  v(Jt)  ~  N(0,  R)  and  is  temporally  white.  Assume  further  that  some  unbiased 
a  priori  statistical  information  about  x(0)  is  available  of  the  form 

xo  ~  N{x(0),Po),  (C.3) 

where  Xo  and  v(it)  are  independent.  The  objective  is  to  determine  how  well  x(A), 
A  >  0,  can  be  estimated  given  the  sequence  of  observations  Zn,  where 

Z];S  {xo,z(0),z(l),---,i(Ar)}.  (C.4) 

Under  the  stated  assumptions  of  independence  and  normal  distributions,  the  joint 
conditional  probability  density  function  of  Z)y  given  x(  A)  can  be  written,  from  (C.2) 
and  (C.3),  as 

(C.5) 

where’ 

A  =  (2x)"/2|Po|‘/*n[(2T)’"/^|R(jb)|’/2],  (C.6) 

’The  notation  |P|  denotes  the  determinant  of  P. 
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and 


*(Zn;x(W))  =  (xo  -  iio)’'Pj'(xo  -  Xo) 

+  EWt)  -  i(t)|’'R-'(t)l^(‘)  -  (C.7) 

k=0 

)Co  =  f {xo}  =  x(0),  and  (C.8) 

i{k)  t  £{i(lt))  =  h(x(t)l.  (C.9) 


The  Cramer-Rao  lower  bound  for  the  error  covariance  matrix  S{N)  of  an  unbiased 
estimator  x{N)  of  the  state  x(N),  where  the  estimate  is  based  on  the  observation 
sequence  Zjy,  is  given  by 


S(N)  =  S{[x{N)  -  x{N)][x{N)  -  x{N)f}  >  3-\N),  (C.IO) 

where  J{N)  is  Fisher’s  information  matrix.  The  matrix  inequality  S  >  J“*  is  equiva¬ 
lent  to  stating  that  (S— J~^)  is  positive  semi-definite.  In  particular,  since  the  diagonal 
elements  of  a  positive  semi-definite  matrix  are  non-negative,  the  diagonal  elements 
of  J~^(JV)  provide  the  estimation  error  variance  lower  bounds  for  the  corresponding 
elements  of  x(7V).  In  the  linear  system  and  measurement  case,  (B.l)  and  (B.3),  the 
discrete  Kalman  filter  given  by  (B.12)  and  (B.13)  provides  the  minimum  variance 
unbiased  estimate.  If  the  system  noise  yf{k)  in  (B.l)  is  zero,  then  for  the  linear  case 


S(^)  =  P{N\N)  =  3-\N).  (C.ll) 

In  the  nonlinear  EKF  formulation,  S{N)  and  P{N\N)  are  not,  in  general,  equivalent. 

Fisher’s  information  measure  on  x(A^)  contained  in  is  given  by  two  equivalent 
representations  [80,  pp.  91-93]: 

where  the  gradient  d\nps/dx{N)  is  a  row  vector.  The  inequality  (C.IO)  with  J(./V) 
expressed  as  in  (C.12)  is  termed  the  "Cramer-Rao  inequality.” 


Taking  the  logarithm  of  pn  in  (C.5)  yields 

lnpx,  = -111.4 -i#|Z]l;x(JV)l. 

The  gradient  of  (C.14)  is  given  by  the  row  vector 

d\npN 


dxiN) 


=  (xo-Xo)^P, 


®  dx{N) 

JtsO 


'dx(Ny 


(C.14) 


(C.15) 


DRES-SR-577 


C.l 


UNCLASSIFIED 

Appendix  C 

Cramer-Rao  Covariance  Bounds 

The  extended  Kalman  filter  is  generally  a  suboptimal  state  estimation  technique. 
A  natural  consideration  in  any  estimation  problem  is  how  well  the  proposed  technique 
performs  when  compared  to  the  optimal  performance  possible.  A  powerful  result 
which  provides  a  performance  assessment  of  parameter  estimation  techniques  is  the 
Cramer-Rao  inequality  [79].  The  following  derivation  draws  from  methods  outlined 
in  [61,62,  74,79,  80]. 

Consider  a  nonlinear  discrete-time  estimation  problem  with  noise-free  system 
model 

x{k  +  l)  =  {[x{k)]  (C.l) 

and  measurement  model 

z(it)  =  h(x(l))  +  v(t;.  (C.2) 

where  v(ib)  ~  N(0,  R)  and  is  temporally  white.  Assume  further  that  some  unbiased 
a  priori  statistical  information  about  x(0)  is  available  of  the  form 

Xo~N(x(0),Po),  (C.3) 

where  Xo  and  v(jfc)  are  independent.  The  objective  is  to  determine  how  well  x(JV), 
N  >0,  can  be  estimated  given  the  sequence  of  observations  where 

Z«=  {x„,z(0),z(l),---,i(W)).  (C.4) 

Under  the  stated  assumptions  of  independence  and  normal  distributions,  the  joint 
conditional  probability  density  function  of  Zh  given  x(yV)  can  be  written,  from  (C.2) 


and  (C.3),  as 

(C.5) 

where* 

A  =  (2x)"/*|Po|*/*n[(2x)"‘/*lR(Jt)|'/»], 

(C.6) 

k=0 

’The  notation  |P|  denotes  the  determinant  of  P. 
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and 


4'[Z;i;x(A')|  =  (xo-XorPo^xo-Xo) 

+ -  mh  (c.t) 

k=0 

Xo  =  ^{xo}  =  x(0),  and  (C.8) 

z(i)  S  f  {z(l)}  =  h(x{i)).  (C.9) 


The  Cramer-Rao  lower  bound  for  the  error  covariance  matrix  S{N)  of  an  unbiased 
estimator  x(A^)  of  the  state  x(N),  where  the  estimate  is  based  on  the  observation 
sequence  is  given  by 


S{N)  =  e{[x{N)  -  x(N)][x{N)  -  x{N)f}  >  J'HN),  (C.IO) 

where  3{N)  is  Fisher’s  information  matrix.  The  matrix  inequality  S  >  J~*  is  equiva¬ 
lent  to  stating  that  (S— J~^)  is  positive  semi-dehnite.  In  particular,  since  the  diagonal 
elements  of  a  positive  semi-definite  matrix  are  non-negative,  the  diagonal  elements 
of  J~^{N)  provide  the  estimation  error  variance  lower  bounds  for  the  corresponding 
elements  of  x{N).  In  the  linear  system  and  measurement  case,  (B.l)  and  (B.3),  the 
discrete  Kalman  filter  given  by  (B.12)  and  (B.13)  provides  the  minimum  variance 
unbiased  estimate.  If  the  system  noise  w(ifc)  in  (B.l)  is  zero,  then  for  the  linear  case 


S(N)  =  P(jV|iV)  =  (C.ll) 

In  the  nonlinear  EKF  formulation,  S{N)  and  P(jVjiV)  are  not,  in  general,  equivalent. 

Fisher’s  information  measure  on  x{N)  contuned  in  Zjlf  is  given  by  two  equivalent 
representations  [80,  pp.  91-93): 


where  the  gradient  dlnps/dx{N)  is  a  row  vector.  The  inequality  (C.IO)  with  J(A^) 
expressed  as  in  (C.12)  is  termed  the  “Cramer-Rao  inequality.” 

Taking  the  logarithm  of  ps  in  (C.5)  yields 


lnp«  =  -li.A-i*I2J;x(W)J. 


(C.14) 


The  gradient  of  (C.14)  is  given  by  the  row  vector 
d  In  pN 


dx{N) 


=  (Xo-Xo)^P, 


Td-I 


®  dx{N) 


'dx{Ny 


(C.15) 
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To  evaluate  (C.12),  consider  the  four  terms  of  the  outer  product  of  (C.15); 

/  dpN  /  dpN  \  _ 

\dx{N))  \MN)) 

•'{|wi)-i(y)rR-(»(0§5)). 

The  conditional  expectation  in  (C.12)  can  be  computed  from  (C.16)  with  the  following 
observations: 


1.  The  Jacobian  matrices  dx^/dx^N)  and  dz{k)(dx{N)  are  deterministic; 

2.  From  (C.2),  (C.8),  and  (C.9),  the  random  variables  (xo  -  Xo)  and  [z(jk)  -  *(ik)], 
k  =  0,1,’  •  •  ,N,  are  independent  with  zero  mean  and  hence  the  second  and 
third  terms  of  (C.16)  do  not  contribute  to  the  ex{>ectation  and  the  product  of 
summations  in  the  last  term  of  (C.16)  reduces  to  a  single  summation  of  products 
with  k  =  j\ 

3.  By  definitions  (C.2)  and  (C.3), 

€  |(xo  -  3^)(xo  -  jto)^}  =  Po  and 

f  {(*(*) -i(lk)l(z(t)-z(i)r}  =  R(t). 


Furthermore,  from  the  definitions  given  in  (C.8)  and  (C.9), 

dxQ  _  5x(0) 
dx{N)  ~ 

and 


dz{k)  __  ah[x(i)] 
dx{N)  ~  dx{N) 


=  H.(fc) 


^(it) 

dxiN)' 


(C.17) 


(C.18) 
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where 

H,(jfc)  ^  ^  .  (C.19) 

^  x=x(*) 

The  subscript  “t”  on  Ht  is  used  to  emphasize  that  (C.19)  is  evaluated  along  the  true 
trajectory  x{k)  rather  than  along  the  estimated  trajectory  x(jb)  as  in  the  usual  EKF 
realization. 

Performing  the  expectation  of  (C.16)  in  accordance  with  (C.12)  and  substituting 
(C.17),  (C.18),  and  (C.19)  yields  Fisher’s  information  matrix: 


(C.20) 


Evaluation  of  the  alternate  representation  for  Fisher’s  information  matrix  in  (C.13) 
is  most  easily  accomplished  through  component-wise  differentiation  of  (C.14).  Con¬ 
sider  a  vector  ^  €  i?"  and  a  scalar-valued  function  .^(^  given  by 

m  =  (c,2i) 

where  F  €  is  positive  definite  symmetric.  Equation  (C.21)  represents  the  general 

form  of  la  pN  in  (C.14)  which  must  be  differentiated.  Provided  the  components  of  i 
admit  second  derivatives  with  respect  to  components  of  a  vector  i?  €  /?", 


(C.22) 


Taking  the  expectation  of  (C.22)  in  accordance  with  (C.13)  in  a  sequence  of  steps 
with  1?  =  x{N)  and  first  with  ^  =  (x©  —  Xq)  and  then  with  (  =  (2(i)  —  z(fc)], 
ib  =  0, 1,2, . . . ,  jV,  which  implies  that  in  all  steps  (  has  zero  mean  and  deterministic 
first  and  second  partial  derivatives,  eliminates  the  second  term  on  the  right  of  (C.22) 
giving,  with  (C.17),  (C.18),  and  (C.19),  the  previous  result  (C.20)  as  expected. 

Finally,  from  (C.l) 


dx(k  -h  1) 
dx(N) 


*i(k) 


dx{k) 


for  0  <  it  <  iV  -  1, 


(C.23) 
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where 


*.w  s  ® 


for  0  <  ik  <  AT  -  1 


(C.24) 


x=x(A;) 


is  evaluated  along  the  true  trajectory  as  shown.  In  particular,  with  I„  representing 
the  n  X  n  identity  matrix, 


_  T 

dx{N)  ^ 


i  n  *‘(0 

t=/v-i 


dx{k) 

dx{Ny 


(C.25) 


where  the  notation  “iO”  indicates  that  the  index  i  decreases  from  left  to  right  in  the 
matrix  product.  The  Jacobian  matrices  in  (C.20)  are  then  computed  as 


dx{k) 

dxiN) 


N-l 


n  Hi)- 


(C.26) 


Substitution  of  (C.26)  into  (C.20)  gives  a  closed-form  expression  for  Fisher’s  in¬ 
formation  matrix  for  the  system  and  measurement  models  defined  in  (C.l)  and  (C.2): 

UN)  =  (n'«r’(o)  Po-'(n»r'(o)  (C.27) 

+  E  f  n'*r‘(i))  H?'(t)R-'(t)H,(t)  (n'  ♦,-'(*))  • 

jb=0  \  IS*  /  \  i=k  / 

A  more  useful  form  for  practical  computation  is  a  recursive  formulation  which  follows 
from  (C.27)  by  inspection: 

1.  Initialization  - 

J(0)  =  Po '  +  Hf  (0)R-'  (0)H.(0);  (C.28) 

2.  Recursive  evaluation  for  it  >  0  - 

}{k  +  1)  =  [*,-'(*)]’^  J(l)  [*.-■(*)]  +  H?'(t)R-'(i)H,(t).  (C.29) 

If  the  information  matrix  J(jk)  remains  singular  in  the  absence  of  prior  information, 
i.e.  =  0,  then  the  system  has  an  unobservable  subspace.  The  converse  is  not 
generally  true,  that  is  the  bounds  may  decrease  even  though  the  system  is  unobserv¬ 
able.  Observability  of  nonlinear  systems,  although  often  difficult  to  establish,  can  be 
investigated  locally  through  methods  from  differential  geometry  [72]. 

The  above  derivation  has  been  given  for  discrete-time,  nonlinear  system  and  mea¬ 
surement  models.  Furthermore,  this  derivation  assumes  an  unbiased  estimation  pro¬ 
cedure.  A  more  general  result  for  an  estimator  with  state  dependent  bias  b(x)  is 
given  by  [78] 

S(Af)  >  (l.  +  J-'  [l.  +  .  (C.30) 
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where  S{N)  is  defined  in  (C.IO)  and  the  Jacobi2in  of  b  is  evaluated  along  the  true 
trajectory.  However,  the  bias  b(x)  is  generally  not  available  in  analytical  form.  Un¬ 
fortunately,  a  state  dependent  bias  can  result  in  higher  or  lower  bounds  in  compau'ison 
to  J~*:  for  example,  consider  b(x)  =  — x/2  which  gives  S  >  while  b(x)  =  x 

gives  S  >  4J“^.  As  a  result,  most  analysis  proceed  under  the  assumption  of  unbiased 
estimation  resulting  in  approximate  covariance  bounds. 

Broida  and  Chellappa  [28,  30,  31,  37],  Young  and  Chellappa  [34],  Taylor  [61],  and 
Chang  [62]  all  employ  Cramer-Rao  analysis  for  filter  performance  evaluation.  Broida 
2md  Chellappa,  and  Young  and  Chellappa  do  not  consider  a  priori  information.  Tay¬ 
lor  does  consider  information  provided  by  the  initial  state  estimate  and  treats  the 
continuous-time  system  case.  In  the  continuous-discrete  Ritering  techniques  of  Sec¬ 
tion  B.3,  results  in  [61]  indicate  that  approximate  covariance  bounds  can  be  computed 
as  in  (C.28)  and  (C.29),  but  with  replaced  with  $<(ib,  Jb  --  1)  defined  in  (2.11). 

The  Cramer  Rao  bounds  shown  in  Section  4.3  do  not  decrease  monotonically.  This 
seems  to  contr^ldict  the  notion  that  as  more  measurements  become  available,  more  and 
more  information  should  be  accumulated  about  the  observed  process  and,  as  a  result, 
the  CRLB’s  would  be  expected  to  decrease.  Fisher’s  information  matrix  defined  in 
(C.12)  provides  a  measure  of  information  contained  in  Zj^  concerning  the  state  x{N). 
As  time,  N,  increases,  the  measurement  set  Zh  accumulates  more  measurements  but, 
in  addition,  the  state  x{N)  may  vary  with  time.  As  discussed  by  Young  and  Chellappa 
[34],  the  noise- free  dynamical  model  implies  a  one-to-one  deterministic  relationship 
between  x(0)  and  x{N)  so  that 


Pz*j,\xiN)  =  P^j;,|x(o), 

(C.31) 

and 

dpzy\x(N)  r5pz3;,|x(o)]  ax(0) 

dx{N)  [  dx{0) 

Inversion  of  Fisher’s  information  matrix  then  gives 

(C.32) 

(C.33) 

where  (')~^  denotes  the  inverse  transpose  and 

I'l" 

(C.34) 

f  ^(0)  \ 

(dx(N)J 

-I 

Cramer  Rao  bounds  for  estimation  of  the  initial  state  x(0)  from  the  measurement 

set  Zff  are  the  diagonal  elements  of  and  these  decrease  monotonically.  The 

bounds  on  x{N),  however,  depend  on  the  time-varying  congruent  transformation  of 
K“‘(Ar)  in  (C.34)  and  need  not  decrease  monatonically. 
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Appendix  D 

Measurement  Model  Jacobian 


Implementation  of  the  extended  Kalman  filter  and  estimation  of  Cramer  Rao 
bounds  requires  the  computation  of  Ie(C)  which  are  all  components  of 

the  measurement  model,  as  well  as  the  Jacobian  of  h[x], 

H(xl  ^  (D.l) 

from  the  most  recent  state  estimate.  Computation  of  H[x]  for  the  three  parameteri- 
zations  of  rotational  motion  is  a  tedious  exercise  in  differentiation,  but,  with  algebraic 
manipulation  and  simplification,  can  be  written  in  simple  form  which  admits  compu¬ 
tationally  efficient  implementation. 

The  Jacobian  matrix,  H  €  can  be  written  as 


Hi  1 


LHyv,  J 


(D.2) 


where  is  the  approximate  linear  measurement  matrix  corresponding  to  the  jth 
camera.  Each  Hj  can  be  further  partitioned  according  to  the  feature  point  of  interest 
as 


1 

H? 

PJ 

0  WJ  0  SJ  0  ••  0 

P? 

0  0  0  S?  0 

•  •  •  •  •  .  • 

pJv/ 

•  •  •  •  .  .  ^ 

0  wf'  0  0  0  • 

where  each  i  =  1,2,...,  Nj,  j  =  1, 2, . . . ,  Ac  is  defined  as 


(D.3) 


(D.4) 


H 


) 


dx 


(D.5) 
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The  submatrices  PJ  €  6 depending  on  the  parameterization 

of  rotational  motion,  and  Sj  €  IR^^^can  be  written,  after  some  aJgebraic  manipula¬ 
tion,  as  follows: 


{D.6) 

(D.7) 

(D.8) 


All  entries  of  H  other  than  those  specified  above  are  zero.  In  the  above  equations, 
3e  =  [0, 1,0]^,  kfi  =  [0,0, 1]^,  and  and  k'g  are  the  matrix  cross  product  operators 
associated  with  }e  and  kjg,  respectively.  In  Equation  (D.7),  the  notation  dlgldC 
denotes  a  three  dimensional  entity  which  premultiplies  a  vector  t?  €  IR^  to  yield  a 
matrix  according  to  the  rule 


fa|0J 

J.j  oCi 


(D.9) 


The  general  form  of  the  measurement  model  is  common  to  all  parameterizations 
of  rotational  motion,  except  that  different  parameterizations  will  yield  different  ex¬ 
pressions  for  These  expressions  are  now  stated  for  each  parameterization. 

Angle- Axis  Parameterization:  The  measurement  model  Jacobian  can  be  com¬ 
puted  with 


dl%{0  _  {1  -co8(7)} 
7 

where 


Pi  +  co8(7)^,^  +  -f-  sin(7)^,  ,  (D.IO) 


7 

A 

(D.ll) 

A 

ii^ir 

(D.12) 

r. 

A 

(D.13) 

Pi 

(D.14) 
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and  denotes  the  jth  component  of  the  unit  vector  and  Cj  denotes  the  jth  standard 
basis  vector  of  IR^.  For  very  small  7,  it  is  apparent  from  (D.10)-(D.14)  that 


I?K) 

31g(0 


I3  +  ^*,  and 


(D.15) 

(D.16) 


This  information  can  be  used  to  eliminate  numerical  difficulties  encountered  in  appli¬ 
cations  when  (D.IO)  must  be  computed  with  7  =  |1{||  ~  0. 

Quaternion  Parameterization:  The  Jacobian  of  the  measurement  model  is 
computed  with 
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94 

93 
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.  92 
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94 

(D.17) 

(D.18) 

and  (D.19) 

(D.20) 


Roll-Pitch- Yaw  Parameterization:  The  measurement  model  Jacobian  is  com¬ 
puted  with 


dmo 

d<f> 


39 


dip 


II 

(D.21) 

-  =  exp(0e3)e2exp(0e*)exp(^J),  and 

(D.22) 

^  =  «5ig. 

(D.23) 

where,  again,  the  ej,  j  =  1,2,3,  are  the  standard  basis  vectors  oflR^. 
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